Comments (24)
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
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付近のように,
速度が0から+になる場合に,節点の速度を出そうとして
オーバーシュートすることになるのを防いでいます.
いまの条件でも例えば水色のデータの1250あたりは
入力(input2.pngの1.0が出力の1000に相当)と比べても
入力との差分が大きくなっています.
ところで,output2-no-if.png を見てみると,
2250あたりで値が富んでいます.どうも不定期に
この現象がでるのですが,心当たりはありますでしょうか?
OpenRTMはどのバージョンをつかっていますでしょうか?
seqplay, interpolatorだけをテストするようなことはされているでしょうか?
- seqplay, interpolatorでエンバグした
- http://openrtm.org/openrtm/ja/content/openrtm-users-02370-1110%E3%81%A7%E3%81%AEopenrtmopenhrp3hrpsys のCOILまわりでメモリーリークしている
- 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から時刻をとれるようにする、というのを1年以上前に安藤さんに提案した覚えがあります。が、おそらく忘れ去られているのでしょうね。
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を使って
貼付のようにしてみました.多分これで大丈夫です.
よろしくお願いいたします.
中西くん,もし,時間があれば試してください.
1.金広さんがパッチをコミットしてくれたら
2.roscd hrpsys/build/hrpsys-base/rtc/SequencePlayer; svn up
3.roscd hrpsys; rm installed; make
で確認してみてください.
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.
Related Issues (20)
- openhrp3が複数存在するとhrpsysが下位のopenhrp3に依存してしまう HOT 8
- 関節角リミットがtableで定義されている関節ではvelocity limit overになることがある HOT 5
- step time validityの閾値
- RT-Linux vs ART-Linux HOT 4
- <setJointAngles/playPattern>OfGroupを一度呼ぶとsetJointAnglesで動かせなくなる HOT 10
- [KalmanFilter]実機のQuaternionExtendedKalmanFilterの姿勢推定結果が正しくない HOT 7
- sequenceplayerの補間queueについて HOT 3
- QNXの環境においてSequencePlayerのsetTargetPoseを呼ぶとコアダンプ HOT 10
- Stabilizerのinportの定義について HOT 1
- [Stabilizer]変数未初期化
- Issue in installation HOT 9
- Is ubuntu 16 supported HOT 3
- Compilation error in ImageData2CameraImage
- make時にBasicDataType.hhが見つからないというエラーが出てしまう HOT 12
- hrpsys-baseと連携させるChoreonoidのバージョンについて HOT 6
- release request
- hrpsys on 20.04 HOT 1
- Problem in Installation in Ubuntu 18.04 HOT 1
- Should Null space be computed using SR-Inverse?
- 【SequencePlayer】setWrenches等にtm=0を与えた場合の挙動のバグについて HOT 2
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.
from hrpsys-base.