From [email protected] on January 02, 2012 17:48:41

経由点での速度も指定した補間方法を教えてもらえませんか？
その場合でも躍度最小になるんでしょうか？

from hrpsys-base.

From [email protected] on January 05, 2012 05:09:18

だいぶ変更点があるのですが，添付のパッチになるかと思います．
www.shadmehrlab.org/book/minimumjerk.pdf
の最後のページでOur conditions at the end of the movement are:
の部分で終端の速度，加速度を与えた形に読み替えて式を解くと
da = -9_(aa-ga_1/3.0)/rm_t - 36_(vv+gv_2/3.0)/(rm_t_rm_t)
+ 60_(gx-xx)/(rm_t_rm_t_rm_t);
になるかと思います．
位置の6階微分が0になるという制約条件は守っているのでmin jerkには
なっているかと思います．

実装ですが，
interpolator.h で

• double *x, *v, *a, *g;
• double *x, *v, *a;
• double *gx, *gv, *ga;
  と言う風にx,v,aのゴールをそれぞれ持つようにしましたが，正しい方向でしょうか？
  現状はgo,setgoal等で速度，加速度の目標をもつ関数と持たない関数を作っていますが
  interpolator, （あるいはseq）のレベルではga,gv,gaを引数に持つ関数だけにしたほうが
  すっきりするかもしれません．

今はi-1,i,i+1の位置から速度を計算していますが，i-2,i-1,iから速度を計算しても
そこそこ上手くいきます． r40 ( http://code.google.com/p/hrpsys-base/source/detail?r=40)で 変更してしまいましたが，動いている途中にゴールを指定したら，いまのinterpolatorの
queueの最後に追加する，という方式をとった場合には，この方法でも上手くいきます．

ここまで書いて思ったんですがaccについても計算して->goに入れてあげる必要があるかもしれません．いまは節点でa=0になっています．

Owner: [email protected]
Cc: [email protected] [email protected] [email protected]

from hrpsys-base.

From [email protected] on January 05, 2012 05:10:32

画像とpatchを添付し忘れました．

Attachment: seqplay_auto_smooth.patch output-smooth.png

from hrpsys-base.

From [email protected] on January 05, 2012 05:22:23

Cc: [email protected]

from hrpsys-base.

From [email protected] on January 05, 2012 18:17:09

パッチありがとうございます。
時刻とポーズの対応関係に誤解があるようです。
pose[i-1]からpose[i]への遷移にかける時間をtime[i-1]と解釈されているようですが、time[i]です。これは修正しておきます。
前後の速度の符号が異なる場合は、その点の通過速度を0としているようですがこれはなぜでしょうか？
他と同じように前後の平均でもいいように思えますが。

from hrpsys-base.

From [email protected] on January 05, 2012 20:59:22

岡田です．

これがないと，
添付のoutput2-no-if.pngの赤い線の3000付近のように，
速度が０から＋になる場合に，節点の速度を出そうとして
オーバーシュートすることになるのを防いでいます．
いまの条件でも例えば水色のデータの1250あたりは
入力(input2.pngの1.0が出力の1000に相当)と比べても
入力との差分が大きくなっています．

ところで，output2-no-if.png を見てみると，
2250あたりで値が富んでいます．どうも不定期に
この現象がでるのですが，心当たりはありますでしょうか？
OpenRTMはどのバージョンをつかっていますでしょうか？
seqplay, interpolatorだけをテストするようなことはされているでしょうか？

1. seqplay, interpolatorでエンバグした
2. http://openrtm.org/openrtm/ja/content/openrtm-users-02370-1110%E3%81%A7%E3%81%AEopenrtmopenhrp3hrpsys のCOILまわりでメモリーリークしている
3. openrtmのリングバッファの問題？？？

Attachment: output2-no-if.png output2-if.png input2.png

from hrpsys-base.

From [email protected] on January 05, 2012 22:13:26

とりあえずパッチをマージしたものを r62 でコミットしました。
値が飛ぶ問題は5回試した限りでは確認できていませんが、どの程度の頻度でしょうか。
データが抜け落ちたりするのであればバッファリングの問題かとも思いますが、おかしな値がでるとなると内部の問題でしょうか。
こちらでは1.0.0のソースにMLで流れたパッチを当てたものを使用しています。1.0.1でも未だ修正されないままになっているバグがあるようなので。

interpolator::get vs = NULL, q.size() = 3878, length = 3878
に関しては以前時々起きていて最近トンと見かけなかったのですがやはりまだ出る場合があるのですね。排他制御を真面目にやっていないのでその辺の問題だと思うのですが、なかなか再現できないので潰せないままになっています。高い頻度で起こす方法があれば教えてください。

hrpsyspyを使わない書き方とは？何のことでしょう。

タイムスタンプをつけるのは構わないですが、どのような仕様にすべきでしょうか？

from hrpsys-base.

From [email protected] on January 05, 2012 23:00:15

値が飛ぶ問題は5回試した限りでは確認できていませんが、どの程度の頻度でしょうか。
現時点で5回試すと1回ぐらいは，おかしな値がでるかvs = NULLかsegfaultします．
昨年はそうではなかった気がするんですが．．．
こちらでは，model loaderを立ち上げたあと，最初に添付したファイルの中にある
./SequencePlayer.sh
./qRefIn.py
hrpsyspy sample.py
を実行して試しています．

こちらでは1.0.0のソースにMLで流れたパッチを当てたものを使用しています
具体的にどのメールになりますでしょうか？ http://openrtm.org/openrtm/ja/content/openrtm-users-01448-mismatched-delete-statemachineh でしょうか？

hrpsyspyを使わない書き方とは？何のことでしょう。
は，sample.pyで入力の値をmatplotlibを使って表示しようとして，
hrpsyspyでは動かなかったので，cpythonで動かしたいなと思いました．

タイムスタンプをつけるのは構わないですが、どのような仕様にすべきでしょうか？

理想を言えば，
simulationの場合，OpenHRP3の画面に出てくるシミュレーションの時刻でいいですが，
実機の場合はRobotHardwareでデータを取得した時刻（seqplayerにトリガを流した
プログラムにトリガがかかった時刻）にするかseqplayerのonExecuteが呼ばれた時刻
（あるいはSeqが出力する時刻）
スタンドアロンでseqplayerが動く場合はseqPlayerのOnExecuteが呼ばれた時刻（あるいは
Seqが出力する時刻）
となるでしょうか？
実行コンテキストが現在の時間をもっていて（実機だと計算機の時間，シミュレーションだと
シミュレーション時間），seqPlayerのonExecuteが呼ばれたとき(or出力するとき）にその時間
を参照して送り出せばいいのかな．どうでしょうか？

from hrpsys-base.

From [email protected] on January 05, 2012 23:52:03

SequencePlayerのテストですが、僕はSequencePlayerとDataLoggerを1つのecで駆動してその結果をファイルに保存して確認しました。

次に1.0.0に加えた修正ですが、ちゃんとした履歴が残っていないのですが、覚えている範囲では以下の3点です。
[openrtm-users 01320]に添付のRingBuffer.h
[openrtm-users 01179]に添付のManager.cpp.diff
PublisherFlush.cppのコンストラクタでm_retcodeをPORT_OKに初期化
1.0.1では未修正だった問題を修正した1.0.2が先程でたらしいのでこれらも直っているかもしれません。

cpythonで使うのであれば、サンプルがないのですが、hrpsys/pythonにPYTHONPATHを通して、
import rtm
from OpenHRP import *
seq = rtm.findRTC("SequencePlayer0")
seq_svc = seq.service("service0")
とかでいけるはずです。

タイムスタンプに関しては同意見で、ECから時刻をとれるようにする、というのを１年以上前に安藤さんに提案した覚えがあります。が、おそらく忘れ去られているのでしょうね。

from hrpsys-base.

From [email protected] on January 06, 2012 01:01:19

ありがとうございます．RingBuffer.hを更新したら変な値が出てくるのは治った気がします．
僕が試しているqRefIn.pyが10hzぐらいでしか動いていないので
(プログラムか設定ファイルが悪い？）SeqPlayerが終わっていても
そこから10秒ぐらい後にならないと補間している部分のデータが
とおり過ぎてくれないという状況でした．

まだ，Manager.cpp.patchを入手できていないからか，
interpolator::get vs = NULL, q.size() = 1203, length = 1203
はでてきます．

hrpsys-test.tgz に含めたqRefIn.pyで，5行目のimport roslibを抜けば
openrtmにPYTHONPATHが通って入れば動くと思うので試していただけるとうれしいです．

from hrpsys-base.

From [email protected] on January 06, 2012 02:42:10

添付されていたサンプルで試したところ、変な値がでる、エラーメッセージが出て落ちる、の両方の現象を確認しました。
まだ原因不明ですが、変な値がでるのはSequencePlayerを出る時点で既にそうなっているので内部の問題のようです。
エラーメッセージが出るのはポインタがNULLになっている場合ですが、それ以外にもおかしなアドレスを指している場合もあるようなので、この2つの問題の原因は同じところにあるのかもしれません。

from hrpsys-base.

From [email protected] on January 06, 2012 06:13:43

状況が伝わってよかったです．

ところで，www.shadmehrlab.org/book/minimumjerk.pdf の4頁目にあるように
逐次的に解くではなく，境界条件から区間内での軌道を解析的に計算する方法が
あって，今のnervousはこちらをつかっているようです．これのメリットは短い
長さのシーケンスの補間でも値が不安定にならないということです．
確かに見ていると，今の実装は経由点に近づくとv,aが不安定になっていますね．
ということで，まだ完全に確認はしていませんが忘れないうちにパッチをおくって
おきます．

Attachment: non-sequential-minjerk.patch

from hrpsys-base.

From [email protected] on January 07, 2012 19:30:57

r63 で頂いたパッチをマージしました。

from hrpsys-base.

From [email protected] on January 09, 2012 19:39:09

r66 でNULLが検出される問題を修正しました。 r40 で補間中に目標を変更できるようにした際に入ったバグでした。
まだ問題が起きるようでしたらお知らせください。

Status: Fixed

from hrpsys-base.

From [email protected] on July 31, 2012 02:36:57

岡田です。

補間中に目標を変更する機能をつかってロボットを動かしているのですが、
ところどころ、目標値が飛ぶ気がしています。
添付の写真を実機のログで緑がエンコーダ（q）、赤が指令値（qRef）になっていて、
浦田ドライバ側でエラーが大きいとサーボオフするので、指令値が飛んだ後から
エンコーダが追従しなくなっています。なにか心当たりがあれば教えてください。
ちなみに、以下のようなプログラムで確認しています。

#!/usr/bin/env python

import roslib
roslib.load_manifest('hrpsys')

import os
import rtm

from rtm import *
from OpenHRP import *

import math,time

seq = rtm.findRTC("seq")
seq_svc = narrow(seq.service("service0"), "SequencePlayerService")

tm = 0
while 1 :
a = 0.5_math.sin(tm)
tm += 0.2
print a
seq_svc.setJointAngles([a,math.pi/2,-math.pi_0.7,0,0,0,0,0],1.0)
time.sleep(0.1)

Status: Started

Attachment: Screenshot-4.png

from hrpsys-base.

From [email protected] on July 31, 2012 03:53:02

この現象は、普通のLinuxでもおきますか？どの程度の頻度でおきますか？
今のところ再現できていません。
原因ですが、非実時間側と実時間側の間の排他制御をやっていないのが原因ではないかと考えています。

from hrpsys-base.

From [email protected] on July 31, 2012 04:07:50

今，通常のlinux+simulationで再現しないか確認中ですが，
実機(1msec)ですと3－5分ぐらいで起こっています．
排他制御は

RTC::ReturnCode_t SequencePlayer::onExecute(RTC::UniqueId ec_id)
{
... isnew...
lock
...
unlock
}

bool SequencePlayer::setJointAngles(const double *angles, double tm)
{
lock
...
unlock
}
みたいな感じでしょうか？
mutexはpthreadを使うのでしょうか？それとも，openrtmで用意されているんでしたっけ？

from hrpsys-base.

From [email protected] on July 31, 2012 04:26:44

とりあえず、こんな感じで入れてみました。現在動作確認中です。

Index: SequencePlayer.cpp

--- SequencePlayer.cpp ( revision 427 )
+++ SequencePlayer.cpp (working copy)
@@ -168,6 +168,8 @@
if (m_qInitIn.isNew()) m_qInitIn.read();
if (m_basePosInitIn.isNew()) m_basePosInitIn.read();
if (m_baseRpyInitIn.isNew()) m_baseRpyInitIn.read();
+

• mutex.lock();

if (m_seq->isEmpty()){
m_clearFlag = false;
@@ -200,6 +202,9 @@
m_seq->clear(0.001);
}
}
+

• mutex.unlock();

return RTC::RTC_OK;
}

@@ -252,6 +257,9 @@
bool SequencePlayer::setJointAngle(short id, double angle, double tm)
{
if (!setInitialState()) return false;
+

• mutex.lock();

dvector q(m_robot->numJoints());
m_seq->getJointAngles(q.data());
q[id] = angle;
@@ -266,12 +274,18 @@
hrp::Vector3 relZmp = root->R.transpose()*(absZmp - root->p);
m_seq->setJointAngles(q.data(), tm);
m_seq->setZmp(relZmp.data(), tm);
+

• mutex.unlock();

return true;
}

bool SequencePlayer::setJointAngles(const double *angles, double tm)
{
if (!setInitialState()) return false;
+

• mutex.lock();

for (int i=0; i<m_robot->numJoints(); i++){
hrp::Link j = m_robot->joint(i);
if (j) j->q = angles[i];
@@ -283,6 +297,9 @@
hrp::Vector3 relZmp = root->R.transpose()(absZmp - root->p);
m_seq->setJointAngles(angles, tm);
m_seq->setZmp(relZmp.data(), tm);
+

• mutex.unlock();

return true;
}

@@ -290,37 +307,67 @@
double tm)
{
if (!setInitialState()) return false;
+

• mutex.lock();

double pose[m_robot->numJoints()];
for (int i=0; i<m_robot->numJoints(); i++){
pose[i] = mask[i] ? angles[i] : m_qInit.data[i];
}
m_seq->setJointAngles(pose, tm);
+

• mutex.unlock();

return true;
}

bool SequencePlayer::setBasePos(const double *pos, double tm)
{
+

• mutex.lock();

m_seq->setBasePos(pos, tm);
+

• mutex.unlock();

return true;
}

bool SequencePlayer::setBaseRpy(const double *rpy, double tm)
{
+

• mutex.lock();

m_seq->setBaseRpy(rpy, tm);
+

• mutex.unlock();

return true;
}

bool SequencePlayer::setZmp(const double *zmp, double tm)
{
+

• mutex.lock();

m_seq->setZmp(zmp, tm);
+

• mutex.unlock();

return true;
}

void SequencePlayer::loadPattern(const char *basename, double tm)
{
+

• mutex.lock();

if (setInitialState()){
m_seq->loadPattern(basename, tm);
}
+

• mutex.unlock();

}

bool SequencePlayer::setInitialState()
@@ -368,6 +415,8 @@
{
if (!setInitialState()) return;

• mutex.lock();

const double *q=NULL, *z=NULL, *a=NULL, *p=NULL, *e=NULL; double t=0;
unsigned int len = (pos.length()>0?pos[0].length():0);
double *v = new double[len];
@@ -406,6 +455,9 @@
t, false);
}
m_seq->sync();
+

• mutex.unlock();

delete [] v;
}

Index: SequencePlayer.h

--- SequencePlayer.h ( revision 427 )
+++ SequencePlayer.h (working copy)
@@ -11,6 +11,7 @@
#define SEQUENCEPLAYER_H

#include <boost/interprocess/sync/interprocess_semaphore.hpp>
+#include <coil/Mutex.h>
#include <rtm/Manager.h>
#include <rtm/DataFlowComponentBase.h>
#include <rtm/CorbaPort.h>
@@ -159,6 +160,7 @@
boost::interprocess::interprocess_semaphore m_waitSem;
hrp::BodyPtr m_robot;
int dummy;

• coil::Mutex mutex;
  };

from hrpsys-base.

From [email protected] on July 31, 2012 05:30:24

はい、（これが原因だとすれば）そんな感じでいいと思いますが、 http://code.google.com/p/hrpsys-base/source/browse/trunk/rtc/DataLogger/DataLogger.cpp でも使っているcoil::Guardを使った方が、unlockをし忘れることがなく、ロックがかかった状態で例外が起きても確実にunlockされるので安全です。

from hrpsys-base.

From [email protected] on July 31, 2012 23:25:10

前のmutexにしたところ大変よくなりました．
また，パッチとしてはGuardを使って
貼付のようにしてみました．多分これで大丈夫です．
よろしくお願いいたします．

中西くん，もし，時間があれば試してください．
１．金広さんがパッチをコミットしてくれたら
２．roscd hrpsys/build/hrpsys-base/rtc/SequencePlayer; svn up
３．roscd hrpsys; rm installed; make
で確認してみてください．

Cc: [email protected]

from hrpsys-base.

From [email protected] on July 31, 2012 23:26:49

添付ファイルがないようです

from hrpsys-base.

From [email protected] on July 31, 2012 23:34:05

失礼しました．

Attachment: SequencePlayer.patch

from hrpsys-base.

From [email protected] on July 31, 2012 23:41:05

r430 で適用しました。

from hrpsys-base.

From [email protected] on March 24, 2013 05:58:56

Status: Fixed

from hrpsys-base.