TrendGraphModel.java
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * General Robotix Inc.
8  * National Institute of Advanced Industrial Science and Technology (AIST)
9  */
10 package com.generalrobotix.ui.view.graph;
11 
12 import java.util.*;
13 
14 import javax.vecmath.AxisAngle4d;
15 import javax.vecmath.Matrix3d;
16 
17 import com.generalrobotix.ui.GrxTimeSeriesItem;
18 import com.generalrobotix.ui.grxui.Activator;
19 import com.generalrobotix.ui.item.GrxWorldStateItem;
20 import com.generalrobotix.ui.item.GrxWorldStateItem.WorldStateEx;
21 import com.generalrobotix.ui.item.GrxWorldStateItem.CharacterStateEx;
22 import com.generalrobotix.ui.util.GrxDebugUtil;
23 import com.generalrobotix.ui.view.graph.LogManager.LogHeader;
24 
31 public class TrendGraphModel
32 {
33  private TrendGraph[] trendGraph_;
34 
35  public static final double TIME_SCALE = 1000000; // タイムカウントの倍率(1μsec)
36  private static final double MAX_DIV = 10; // 時間軸の最大分割数
37  private static final double LOG10 = Math.log(10);
38 
39  private long stepTimeCount_; // 時間刻み幅(カウント)
40 
41  private double stepTime_; // 時間刻み幅(秒)
42  private double totalTime_; // 総時間(秒)
43  private double currentTime_; // 現在時刻(秒)
44 
45  private double timeRange_; // 時間レンジ(秒)
46  private double markerPos_; // 現在時刻マーカ位置(0.0から1.0で指定)
47 
48  private double baseTime_; // グラフ左端時刻(秒)
49 
50  public int sampleCount_; // グラフサンプル数
51  public long baseCount_; // データ開始位置
52 
53  private AxisInfo timeAxisInfo_; // 時間軸情報
54 
55  private HashMap<String, Integer > dataItemCount_; // データアイテムカウンタ
56  // (どのデータ系列にいくつのデータアイテムが割り当てられているか)
57 
58  private HashMap<String, DataModel> dataModelMap_; // データモデル一覧
59  private DataModel[] dataModelArray_; // データモデル一覧
60  private HashMap<String, AttitudeDataModel> attitudeDataModelMap_;
61 
62  private GrxWorldStateItem world_ = null;
63 
64  private boolean markerFixed_; // 現在時刻マーカ固定フラグ
65  private double fixedMarkerPos_; // 固定マーカ位置
66 
67  //private int mode_; // モード
68  private GraphPanel gPanel_= null;
69 
70  private long prevLogSize_ = -1;
71 
75  public TrendGraphModel(int numGraph) {
76 
77  // 軸情報生成
78  timeAxisInfo_ = new AxisInfo(0,0);
79  timeAxisInfo_.min = 0;
80  timeAxisInfo_.minLimitEnabled = true;
81  timeAxisInfo_.maxLimitEnabled = true;
82  timeAxisInfo_.unitXOfs = 12;
83  timeAxisInfo_.unitYOfs = 0;
84  timeAxisInfo_.unitLabel = "(sec)";
85  timeAxisInfo_.markerColor = Activator.getDefault().getColor( "markerColor" );
86  timeAxisInfo_.markerVisible = true;
87 
88  dataItemCount_ = new HashMap<String, Integer >();
89  dataModelMap_ = new HashMap<String, DataModel>();
90  attitudeDataModelMap_ = new HashMap<String, AttitudeDataModel>();
91  dataModelArray_ = null;
92 
93  trendGraph_ = new TrendGraph[numGraph];
94  for (int i = 0; i < trendGraph_.length; i ++) {
95  StringBuffer graphName = new StringBuffer("Graph");
96  graphName.append(i);
97  trendGraph_[i] = new TrendGraph(this, graphName.toString());
98  }
99  }
100 
101  public int getNumGraph() {
102  return trendGraph_.length;
103  }
104 
105  public TrendGraph getTrendGraph(int index) {
106  return trendGraph_[index];
107  }
108 
109  public String getTrendGraphName(int index) {
110  if (index < 0 || index >= trendGraph_.length) {
111  throw new ArrayIndexOutOfBoundsException();
112  }
113  StringBuffer graphName = new StringBuffer("Graph");
114  graphName.append(index);
115  return graphName.toString();
116  }
117 
118  public void worldTimeChanged(Time time) {
119  setCurrentTime(time.getUtime());
120  for (int i = 0; i < trendGraph_.length; i ++) {
121  trendGraph_[i].repaint();
122  }
123  }
124 
125  // -----------------------------------------------------------------
126  // メソッド
132  public AxisInfo getTimeAxisInfo() {
133  return timeAxisInfo_;
134  }
135 
141  public void setStepTime(
142  long stepTime
143  ) {
144  stepTimeCount_ = stepTime;
145  stepTime_ = stepTimeCount_ / TIME_SCALE;
146 
147  sampleCount_ = (int)Math.floor(timeRange_ / stepTime_) + 2; // サンプル数(前後2サンプルを追加)
148  baseCount_ = Math.round(baseTime_ / stepTime_); //- 1; // データ開始位置
149 
150  // 全データ系列の更新
151  Iterator<DataModel> itr = dataModelMap_.values().iterator();
152  while (itr.hasNext()) {
153  DataModel dm = (DataModel)itr.next();
154  dm.dataSeries.setSize(sampleCount_);
155  dm.dataSeries.setXStep(stepTime_);
156  dm.dataSeries.setXOffset(baseCount_);
157  }
158  Iterator<AttitudeDataModel> itr0 = attitudeDataModelMap_.values().iterator();
159  while (itr0.hasNext()) {
160  AttitudeDataModel ad = itr0.next();
161  for(int i=0; i<3; i++){
162  if(ad.rpySeries[i]!=null){
163  ad.rpySeries[i].setSize(sampleCount_);
164  ad.rpySeries[i].setXStep(stepTime_);
165  ad.rpySeries[i].setXOffset(baseCount_);
166  }
167  }
168  }
169  }
170 
176  public void setTotalTime(double totalTime) {
177  totalTime_ = totalTime;
178  timeAxisInfo_.max = totalTime_;
179  }
180 
187  public void setRangeAndPos(
188  double timeRange,
189  double markerPos
190  ) {
191  timeRange_ = timeRange;
192  fixedMarkerPos_ = markerPos;
193  timeAxisInfo_.extent = timeRange_;
194  _updateDiv();
195 
196  init();
197  }
198 
204  public double getTimeRange() {
205  return timeRange_;
206  }
207 
213  public double getMarkerPos() {
214  return fixedMarkerPos_;
215  }
216 
222  public double getStepTime() {
223  return stepTime_;
224  }
225 
231  public double getTotalTime() {
232  return totalTime_;
233  }
234 
240  public void setCurrentTime(long currentTime) {
241  currentTime_ = currentTime / TIME_SCALE;
242 
243  long oldBaseCount = baseCount_;
244  long totalCount = Math.round(totalTime_ / stepTime_);
245  markerFixed_ = (timeRange_ * fixedMarkerPos_ < totalTime_);
246  if (markerFixed_) {
247  markerPos_ = fixedMarkerPos_;
248  baseTime_ = currentTime_ - timeRange_ * markerPos_; // グラフ左端位置
249  baseCount_ = Math.round(baseTime_ / stepTime_); // - 1; // データ開始位置
250  } else {
251  markerPos_ = currentTime_ / timeRange_;
252  baseTime_ = 0;
253  baseCount_ = 0;
254  }
255 
256  timeAxisInfo_.markerPos = markerPos_;
257  timeAxisInfo_.base = baseTime_;
258 
259  if (dataModelArray_ == null) {
260  return;
261  }
262 
263  if(totalCount==0)
264  return;
265 
266  // データを読み直す
267 
268  int diff = (int)(baseCount_ - oldBaseCount);
269 
270  // 全データ系列の移動
271  Iterator<DataModel> itr = dataModelMap_.values().iterator();
272  while (itr.hasNext()) {
273  DataSeries ds = ((DataModel)itr.next()).dataSeries;
274  ds.shift(diff);
275  }
276  Iterator<AttitudeDataModel> itr0 = attitudeDataModelMap_.values().iterator();
277  while (itr0.hasNext()) {
278  AttitudeDataModel ad = itr0.next();
279  for(int i=0; i<3; i++){
280  if(ad.rpySeries[i]!=null){
281  ad.rpySeries[i].shift(diff);
282  }
283  }
284  }
285 
286  if(prevLogSize_ < totalCount)
287  {
288  int yet = (int)(baseCount_ + sampleCount_ - prevLogSize_);
289  if(diff < yet)
290  diff = yet;
291  }
292  prevLogSize_ = totalCount;
293 
294  if (diff > 0) {
295  if (diff >= sampleCount_) {
296  _getData(baseCount_, 0, sampleCount_);
297  } else {
298  _getData(baseCount_, sampleCount_ - diff, diff);
299  }
300  } else {
301  if (-diff >= sampleCount_) {
302  _getData(baseCount_, 0, sampleCount_);
303  } else {
304  _getData(baseCount_, 0, -diff);
305  }
306  }
307  }
308 
315  public DataSeries addDataItem(
316  DataItem dataItem
317  ) {
318  DataSeries ds;
319  DataModel dm;
320 
321  String key = dataItem.toString();
322  Integer l = dataItemCount_.get(key);
323  if (l == null) { // 初めてのデータアイテム?
324  if(key.contains("attitude")){
325  AttitudeDataModel attitudeDataModel = attitudeDataModelMap_.get(dataItem.getAttributePath());
326  if(attitudeDataModel==null){
327  attitudeDataModel = new AttitudeDataModel();
328  attitudeDataModelMap_.put(dataItem.getAttributePath(),attitudeDataModel);
329  }
330  ds = new DataSeries(sampleCount_, baseCount_ * stepTime_, stepTime_ );
331  attitudeDataModel.setRPYSeries(dataItem.index, ds);
332  for(int i=0; i<4; i++){
333  DataItem di = new DataItem(dataItem.object, dataItem.node, dataItem.attribute, i, "");
334  if(dataModelMap_.get(di.toString())==null){
335  DataSeries dataSeries = new DataSeries(sampleCount_, baseCount_ * stepTime_, stepTime_ );
336  dm = new DataModel(di, dataSeries);
337  dataModelMap_.put(di.toString(), dm);
338  attitudeDataModel.setAxisAngleSeries(i,dataSeries);
339  }
340  }
341  dataItemCount_.put(key, new Integer(1));
342  }else{
343  dataItemCount_.put(key, new Integer(1));
344  ds = new DataSeries(
345  sampleCount_,
346  baseCount_ * stepTime_, // baseTime_, ★これではダメ
347  stepTime_
348  );
349  dm = new DataModel(dataItem, ds);
350  dataModelMap_.put(key, dm);
351  }
352  dataModelArray_ = new DataModel[dataModelMap_.size()];
353  dataModelMap_.values().toArray(dataModelArray_);
354  } else {
355  dataItemCount_.put(key, ++l);
356  if(key.contains("attitude"))
357  ds = attitudeDataModelMap_.get(dataItem.getAttributePath()).rpySeries[dataItem.index];
358  else
359  ds = ((DataModel)dataModelMap_.get(key)).dataSeries;
360  }
361 
362  initGetData();
363  prevLogSize_ = -1;
364  return ds;
365  }
366 
372  public void removeDataItem(
373  DataItem dataItem
374  ) {
375  String key = dataItem.toString();
376  Integer l = dataItemCount_.get(key); // カウント取得
377  dataItemCount_.put(key, --l); // カウントを減らす
378  if(key.contains("attitude")){
379  if( l<= 0 ){
380  dataItemCount_.remove(key);
381  attitudeDataModelMap_.get(dataItem.getAttributePath()).setRPYSeries(dataItem.index, null);
382  }
383  AttitudeDataModel attitudeDataModel = attitudeDataModelMap_.get(dataItem.getAttributePath());
384  if(attitudeDataModel.isDisused()){
385  attitudeDataModelMap_.remove(dataItem.getAttributePath());
386  for(int i=0; i<4; i++)
387  dataModelMap_.remove(dataItem.getAttributePath()+"."+i);
388  int size = dataModelMap_.size();
389  if (size <= 0) {
390  dataModelArray_ = null;
391  } else {
392  dataModelArray_ = new DataModel[size];
393  dataModelMap_.values().toArray(dataModelArray_);
394  }
395  }
396  }else{
397  if (l <= 0) { // データ系列に対応するデータアイテムがなくなった?
398  dataItemCount_.remove(key); // カウント除去
399  dataModelMap_.remove(key); // データ系列除去
400  int size = dataModelMap_.size();
401  if (size <= 0) {
402  dataModelArray_ = null;
403  } else {
404  dataModelArray_ = new DataModel[size];
405  dataModelMap_.values().toArray(dataModelArray_);
406  }
407  }
408  }
409  initGetData();
410  prevLogSize_ = -1;
411  }
412 
413  // -----------------------------------------------------------------
414  // プライベートメソッド
419  private void _updateDiv() {
420  double sMin = timeAxisInfo_.extent / MAX_DIV;
421  int eMin = (int)Math.floor(Math.log(sMin) / LOG10);
422  double step = 0;
423  String format = "0";
424  int e = eMin;
425  boolean found = false;
426  while (!found) {
427  int m = 1;
428  for (int i = 1; i <= 3; i++) {
429  step = m * Math.pow(10.0, e);
430  if (sMin <= step) { // && step <= sMax) {
431  if (e < 0) {
432  char[] c = new char[-e + 2];
433  c[0] = '0';
434  c[1] = '.';
435  for (int j = 0; j < -e; j++) {
436  c[j + 2] = '0';
437  }
438  format = new String(c);
439  }
440  found = true;
441  break;
442  }
443  m += (2 * i - 1);
444  }
445  e++;
446  }
447  timeAxisInfo_.tickEvery = step;
448  timeAxisInfo_.labelEvery = step;
449  timeAxisInfo_.gridEvery = step;
450  timeAxisInfo_.labelFormat = format;
451  }
452 
453  public void setWorldState(GrxWorldStateItem world) {
454  world_ = world;
455  init();
456  }
457 
458  // 初期化関数　　//
459  private void init(){
460  baseTime_ = 0;
461  currentTime_ = 0;
462  baseCount_ = 0;
463  prevLogSize_ = -1;
464  if(world_ != null){
465  Double time = ((GrxTimeSeriesItem)world_).getTime(world_.getLogSize()-1);
466  if(time != null)
467  setTotalTime(time);
468  else
469  setTotalTime(0);
470  try {
471  double step = world_.getDbl("logTimeStep", 0.001);
472  setStepTime((long)(1000000*step));
473  } catch (Exception e) {
474  GrxDebugUtil.printErr("Couldn't parse log step time.", e);
475  }
476  initGetData();
477  }else{
478  setTotalTime(0);
479  setStepTime(1000);
480  }
481  }
482 
483  public void initGetData(){
484  if(dataModelArray_ != null && world_ != null)
485  if (world_.isUseDsik()){
486  world_.logger_.initGetData(dataModelArray_);
487  }
488  }
489 
490  private void _getData(long origin, int offset, int count){
491  if(world_ == null){
492  return;
493  } else if (world_.isUseDsik()){
494  world_.logger_.getData(origin, offset, count);
495  }else{
496  int changePos = world_.getChangePosition();
497  int counter = changePos - ((int)origin + offset);
498 
499  // ログの記録方式がメモリ方式からファイル方式へスイッチした場合処理
500  if( changePos >= 0 ){
501  if( counter <= 0 ){
502  world_.logger_.getData(-offset-counter, offset, count, dataModelArray_);
503  return;
504  }
505  //ログの記録方式がファイル方式とメモリ方式で混在する場合の境界処理
506  if( counter <= count ){
507  count -= counter;
508  world_.logger_.getData(-offset-counter, offset + counter, count, dataModelArray_);
509  count = counter;
510  }
511  }
512 
513  WorldStateEx refWorld = world_.getValue( (int)origin + offset );
514  if(refWorld == null){
515  for(int h = 0; h < dataModelArray_.length; ++h){
516  DataSeries ds = dataModelArray_[h].dataSeries;
517  double[] dbArray = ds.getData();
518  int localOffset = (ds.getHeadPos() + offset) % dbArray.length;
519  for (int i = 0; i < count; ++i, ++localOffset){
520  if(localOffset +i >= dbArray.length){
521  localOffset = 0;
522  }
523  dbArray[localOffset] = Double.NaN;
524  }
525  }
526  } else {
527  // メモリーデータから表示したいラインの要素を抽出する処理
528  for(int h = 0; h < dataModelArray_.length; ++h){
529  long recNo = origin + offset; // レコード番号
530  DataItem localDataItem = dataModelArray_[h].dataItem;
531  CharacterStateEx refCharacter = refWorld.get(localDataItem.object);
532  CharacterStateEx localCharacter = refCharacter;
533  DataSeries ds = dataModelArray_[h].dataSeries;
534  double[] dbArray = ds.getData();
535  int localOffset = (ds.getHeadPos() + offset) % dbArray.length;
536  int index = world_.logger_.getIndex(localDataItem.object,
537  localDataItem.node + "." + localDataItem.attribute + (localDataItem.index >= 0 ? "." + localDataItem.index : "") );
538  ArrayList<Integer> arryLength = new ArrayList<Integer>();
539  arryLength.add(refCharacter.position.length * 7);
540  arryLength.add(arryLength.get(0) + refCharacter.sensorState.q.length);
541  arryLength.add(arryLength.get(1) + refCharacter.sensorState.u.length);
542  if(refCharacter.sensorState.force.length == 0){
543  arryLength.add( arryLength.get(2));
544  } else {
545  arryLength.add( arryLength.get(2) +
546  refCharacter.sensorState.force.length * refCharacter.sensorState.force[0].length);
547  }
548 
549  if(refCharacter.sensorState.rateGyro.length == 0){
550  arryLength.add( arryLength.get(3));
551  } else {
552  arryLength.add( arryLength.get(3) +
553  refCharacter.sensorState.rateGyro.length * refCharacter.sensorState.rateGyro[0].length);
554  }
555 
556  if(refCharacter.sensorState.accel.length == 0){
557  arryLength.add( arryLength.get(4));
558  } else {
559  arryLength.add( arryLength.get(4) +
560  refCharacter.sensorState.accel.length * refCharacter.sensorState.accel[0].length);
561  }
562  if(refCharacter.sensorState.range.length == 0){
563  arryLength.add( arryLength.get(5));
564  } else {
565  arryLength.add( arryLength.get(5) +
566  refCharacter.sensorState.range.length * refCharacter.sensorState.range[0].length);
567  }
568 
569  for (int i = 0; i < count; ++i, ++recNo, ++localOffset){
570  if(localOffset >= dbArray.length){
571  localOffset = 0;
572  }
573  if(recNo < 0 || recNo >= world_.getLogSize() || localCharacter == null){
574  dbArray[localOffset] = Double.NaN;
575  continue;
576  }
577  if (index <= arryLength.get(0)){
578 
579 
580  } else if (index <= arryLength.get(2)){
581  if( (index - 1) % 2 == 0 ){
582  dbArray[localOffset] = localCharacter.sensorState.q[(index - arryLength.get(0) - 1)/2];
583  } else {
584  dbArray[localOffset] = localCharacter.sensorState.u[(index - arryLength.get(0) - 2)/2];
585  }
586  } else if (index <= arryLength.get(3) && localCharacter.sensorState.force.length > 0){
587  int dim1 = (index - arryLength.get(2) - 1) / localCharacter.sensorState.force[0].length ;
588  int dim2 = (index - arryLength.get(2) - 1) % localCharacter.sensorState.force[0].length ;
589  dbArray[localOffset] = localCharacter.sensorState.force[dim1][dim2];
590 
591  } else if (index <= arryLength.get(4) && localCharacter.sensorState.rateGyro.length > 0){
592  int dim1 = (index - arryLength.get(3) - 1) / localCharacter.sensorState.rateGyro[0].length ;
593  int dim2 = (index - arryLength.get(3) - 1) % localCharacter.sensorState.rateGyro[0].length ;
594  dbArray[localOffset] = localCharacter.sensorState.rateGyro[dim1][dim2];
595  } else if (index <= arryLength.get(5) && localCharacter.sensorState.accel.length > 0){
596  int dim1 = (index - arryLength.get(4) - 1) / localCharacter.sensorState.accel[0].length ;
597  int dim2 = (index - arryLength.get(4) - 1) % localCharacter.sensorState.accel[0].length ;
598  dbArray[localOffset] = localCharacter.sensorState.accel[dim1][dim2];
599  } else if(localCharacter.sensorState.range.length > 0) {
600  int dim1 = (index - arryLength.get(5) - 1) / localCharacter.sensorState.range[0].length ;
601  int dim2 = (index - arryLength.get(5) - 1) % localCharacter.sensorState.range[0].length ;
602  dbArray[localOffset] = localCharacter.sensorState.range[dim1][dim2];
603  }
604  localCharacter = world_.getValue( (int)recNo ).get(localDataItem.object);
605  }
606  }
607  }
608  }
609 
610  //姿勢データをロール、ピッチ、ヨーに変換 //
611  Iterator<AttitudeDataModel> it = attitudeDataModelMap_.values().iterator();
612  while(it.hasNext()){
613  AttitudeDataModel attitudeDataModel = it.next();
614  double[][] aa = new double[4][];
615  int[] aapos = new int[4];
616  int[] aaSize = new int[4];
617  for(int i=0; i<4; i++){
618  DataSeries ds = attitudeDataModel.axisAngleSeries[i];
619  aaSize[i] = ds.getSize();
620  aapos[i] = (ds.getHeadPos() + offset) % aaSize[i];
621  aa[i] = attitudeDataModel.axisAngleSeries[i].getData();
622  }
623  double[][] rpy = {null, null, null};
624  int[] rpypos = new int[3];
625  int[] rpySize = new int[3];
626  for(int i=0; i<3; i++){
627  DataSeries ds = attitudeDataModel.rpySeries[i];
628  if(ds!=null){
629  rpySize[i] = ds.getSize();
630  rpypos[i] = (ds.getHeadPos() + offset) % rpySize[i];
631  rpy[i] = attitudeDataModel.rpySeries[i].getData();
632  }
633  }
634  for (int i = 0; i < count; i++) {
635  if(aa[0][aapos[0]] == Double.NaN || aa[1][aapos[1]] == Double.NaN || aa[2][aapos[2]] == Double.NaN || aa[3][aapos[3]] == Double.NaN){
636  if(rpy[0]!=null) rpy[0][rpypos[0]] = Double.NaN;
637  if(rpy[1]!=null) rpy[1][rpypos[1]] = Double.NaN;
638  if(rpy[2]!=null) rpy[2][rpypos[2]] = Double.NaN;
639  }else
640  AxisAngleToRPY(aa, rpy, aapos, rpypos);
641  for(int j=0; j<4; j++){
642  if(aapos[j] < aaSize[j]-1)
643  aapos[j]++;
644  else
645  aapos[j]=0;
646  }
647  for(int j=0; j<3; j++){
648  if(rpypos[j] < rpySize[j]-1)
649  rpypos[j]++;
650  else
651  rpypos[j]=0;
652  }
653  }
654  }
655  }
656 
657  public void updateGraph(){
658  if(world_!=null){
659  Double time = world_.getTime();
660  if(time!=null)
661  setCurrentTime(new Time(time).getUtime());
662  else
663  setCurrentTime(0);
664  }else
665  setCurrentTime(0);
666  gPanel_.redraw(gPanel_.getLocation().x,gPanel_.getLocation().y,gPanel_.getSize().x,gPanel_.getSize().y,true);
667  }
668 
669  public void clearDataItem() {
670  for (int i = 0; i < getNumGraph(); i++) {
671  TrendGraph t = getTrendGraph(i);
672  t.clearDataItem();
673  }
674  }
675 
676  public void setPanel(GraphPanel gPanel) {
677  gPanel_ = gPanel;
678 
679  }
680 
681  private void AxisAngleToRPY(double[][] aa, double[][] rpy, int[] aapos, int[] rpypos){
682  Matrix3d m = new Matrix3d();
683  m.set(new AxisAngle4d(aa[0][aapos[0]], aa[1][aapos[1]], aa[2][aapos[2]], aa[3][aapos[3]]));
684 
685  // hrpUtil/TVmet3d.cpp からコピー //
686  double roll, pitch, yaw;
687  if ((Math.abs(m.m00)<Math.abs(m.m20)) && (Math.abs(m.m10)<Math.abs(m.m20))) {
688  double sp = -m.m20;
689  if (sp < -1.0) {
690  sp = -1;
691  } else if (sp > 1.0) {
692  sp = 1;
693  }
694  pitch = Math.asin(sp); // -pi/2< p < pi/2
695 
696  roll = Math.atan2(sp*m.m01+m.m12, // -cp*cp*sr*cy
697  sp*m.m02-m.m11); // -cp*cp*cr*cy
698 
699  if (m.m00>0.0) { // cy > 0
700  if(roll < 0.0)
701  roll += Math.PI;
702  else
703  roll -= Math.PI;
704  }
705  double sr=Math.sin(roll), cr=Math.cos(roll);
706  if (sp > 0.0) {
707  yaw = Math.atan2(sr*m.m11+cr*m.m12, //sy*sp
708  sr*m.m01+cr*m.m02);//cy*sp
709  } else {
710  yaw = Math.atan2(-sr*m.m11-cr*m.m12,
711  -sr*m.m01-cr*m.m02);
712  }
713  } else {
714  yaw = Math.atan2(m.m10, m.m00);
715  double sa = Math.sin(yaw);
716  double ca = Math.cos(yaw);
717  pitch = Math.atan2(-m.m20, ca*m.m00+sa*m.m10);
718  roll = Math.atan2(sa*m.m02-ca*m.m12, -sa*m.m01+ca*m.m11);
719  }
720 
721  if(rpy[0]!=null) rpy[0][rpypos[0]] = roll;
722  if(rpy[1]!=null) rpy[1][rpypos[1]] = pitch;
723  if(rpy[2]!=null) rpy[2][rpypos[2]] = yaw;
724  }
725 
726  private class AttitudeDataModel{
727  private DataSeries[] rpySeries = {null, null, null};
728  private DataSeries[] axisAngleSeries = {null, null, null, null};
729 
730  public AttitudeDataModel(){
731  }
732 
733  private void setAxisAngleSeries(int i, DataSeries dataSeries){
734  axisAngleSeries[i] = dataSeries;
735  }
736 
737  private void setRPYSeries(int i, DataSeries dataSeries){
738  rpySeries[i] = dataSeries;
739  }
740 
741  private boolean isDisused(){
742  return rpySeries[0]==null && rpySeries[1]==null && rpySeries[2]==null ;
743  }
744  }
745 }
com.generalrobotix.ui.view.graph.TrendGraphModel.AxisAngleToRPY
void AxisAngleToRPY(double[][] aa, double[][] rpy, int[] aapos, int[] rpypos)
Definition: TrendGraphModel.java:681
autoplay.c
int c
Definition: autoplay.py:16
null
#define null
our own NULL pointer
Definition: IceTypes.h:57
size
png_uint_32 size
Definition: png.h:1521
for
#define for
Definition: IcePreprocessor.h:129
i
png_uint_32 i
Definition: png.h:2735
index
list index
t
t
com.generalrobotix.ui.view.graph.TrendGraphModel._getData
void _getData(long origin, int offset, int count)
Definition: TrendGraphModel.java:490
com.generalrobotix.ui.util.GrxConfigBundle.getDbl
final Double getDbl(String key, Double defaultVal)
get double value associated to key
Definition: GrxConfigBundle.java:212
com.generalrobotix.ui.view.graph.TrendGraphModel.setRangeAndPos
void setRangeAndPos(double timeRange, double markerPos)
Definition: TrendGraphModel.java:187
int
typedef int
Definition: png.h:1113
com.generalrobotix.ui.view.graph.TrendGraphModel.attitudeDataModelMap_
HashMap< String, AttitudeDataModel > attitudeDataModelMap_
Definition: TrendGraphModel.java:60

openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:05