Beispiel #1
0
        public static void SetupModel(ImuzDraw draw, KinematicsViewModel model)
        {
            foreach (Shape s in model.Shape)
            {
                switch (s.Type)
                {
                case ShapeType.Pyramid:
                    draw.AddPyramidModel((float)s.Scale.x, (float)s.Scale.y, (float)s.Scale.z);
                    break;

                default:
                    continue;
                }
            }
        }
Beispiel #2
0
        private void Form1_Load(object sender, EventArgs e)
        {
            _estimator = new CompositePoseEstimator[numberOfLink];

            // フィルタ:速い動きに追従する設定です
            var fs     = 1 / 0.01;
            var cutoff = 25.0;
            var tc     = 0.4;

            for (int i = 0; i < numberOfLink; i++)
            {
                _estimator[i] = new CompositePoseEstimator();
                _estimator[i].SetFilterCoeffs(CompositePoseEstimator.FilterName.AccLowPassFilter, FilterDisignerBiquad.LowPass(fs, cutoff, 1.0));
                _estimator[i].SetFilterCoeffs(CompositePoseEstimator.FilterName.CompLowPassFilter, FilterDisignerBiquad.LowPass(fs, cutoff, 1.0));
                _estimator[i].SetFilterCoeffs(CompositePoseEstimator.FilterName.GyroLowPassFilter, FilterDisignerBiquad.LowPass(fs, 45.0, 1.0));
                _estimator[i].TimeConstantComposition = tc;
            }

            // 接続処理
            var dialog = new PortOpenDialog();

            if (dialog.ShowDialog() != System.Windows.Forms.DialogResult.OK)
            {
                return;
            }

            _port = CommunicationManager.CreateCommunicationPort(dialog.ConnectionNameType);
            // データ更新イベントの登録
            _port.MsgMeasurementReceived += new ReceivedMsgMeasurementHandler(_port_MsgMeasurementReceived);

            dialog.Dispose();
            _port.Open();


            #region 形状、位置設定
            // 骨盤
            _linkPelvis = new Link()
            {
                LinkId       = 0,
                ParentLinkId = -1,
                Position     = new Vec4(),
                Orientation  = Quaternion.CreateFromAngle(0, 0, 0),
            };
            var _linkPelvisShape = new Shape()
            {
                Type        = ShapeType.Pyramid,
                Scale       = new Vec4(30.7 / scale, 3 / scale, 10 / scale, 0 / scale),
                Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0),
                Position    = new Vec4(0, 0, 0, 0),
            };
            _allLinkes.Add(_linkPelvis);
            _allShapes.Add(_linkPelvisShape);

            // 左大腿
            _linkLeftThigh = new Link()
            {
                LinkId       = 1,
                ParentLinkId = 0,
                Position     = new Vec4(15.4 / scale, 0 / scale, 0 / scale),
                Orientation  = Quaternion.CreateFromAngle(0, 0, 0),
            };
            var _linkLeftThighShape = new Shape()
            {
                Type        = ShapeType.Pyramid,
                Scale       = new Vec4(-38.1 / scale, 5 / scale, 3 / scale, 0 / scale),
                Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0),
                Position    = new Vec4(0, 0, 0, 0),
            };
            _allLinkes.Add(_linkLeftThigh);
            _allShapes.Add(_linkLeftThighShape);

            // 左脛
            _linkLeftShin = new Link()
            {
                LinkId       = 2,
                ParentLinkId = 1,
                Position     = new Vec4(0 / scale, 0 / scale, -38.1 / scale),
                Orientation  = Quaternion.CreateFromAngle(0, 0, 0),
            };
            var _linkLeftShinShape = new Shape()
            {
                Type        = ShapeType.Pyramid,
                Scale       = new Vec4(-46.5 / scale, 5 / scale, 3 / scale, 0 / scale),
                Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0),
                Position    = new Vec4(0, 0, 0, 0),
            };
            _allLinkes.Add(_linkLeftShin);
            _allShapes.Add(_linkLeftShinShape);

            // 左足首
            _linkLeftFoot = new Link()
            {
                LinkId       = 3,
                ParentLinkId = 2,
                Position     = new Vec4(0 / scale, 0 / scale, -46.5 / scale),
                Orientation  = Quaternion.CreateFromAngle(0, 0, 0),
            };
            var _linkLeftFootShape = new Shape()
            {
                Type        = ShapeType.Pyramid,
                Scale       = new Vec4(16.5 / scale, 2 / scale, 2 / scale, 0 / scale),
                Orientation = Quaternion.CreateFromAngle(0, Math.PI / 2, -Math.PI / 2),
                Position    = new Vec4(0, 0, 0, 0),
            };
            _allLinkes.Add(_linkLeftFoot);
            _allShapes.Add(_linkLeftFootShape);


            // 右大腿
            _linkRightThigh = new Link()
            {
                LinkId       = 4,
                ParentLinkId = 0,
                Position     = new Vec4(-15.4 / scale, 0 / scale, 0 / scale),
                Orientation  = Quaternion.CreateFromAngle(0, 0, 0),
            };
            var _linkRightThighShape = new Shape()
            {
                Type        = ShapeType.Pyramid,
                Scale       = new Vec4(-38.1 / scale, 5 / scale, 3 / scale, 0 / scale),
                Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0),
                Position    = new Vec4(0, 0, 0, 0),
            };
            _allLinkes.Add(_linkRightThigh);
            _allShapes.Add(_linkRightThighShape);

            // 右脛
            _linkRightShin = new Link()
            {
                LinkId       = 5,
                ParentLinkId = 4,
                Position     = new Vec4(0 / scale, 0 / scale, -38.1 / scale),
                Orientation  = Quaternion.CreateFromAngle(0, 0, 0),
            };
            var _linkRightShinShape = new Shape()
            {
                Type        = ShapeType.Pyramid,
                Scale       = new Vec4(-46.5 / scale, 5 / scale, 3 / scale, 0 / scale),
                Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0),
                Position    = new Vec4(0, 0, 0, 0),
            };
            _allLinkes.Add(_linkRightShin);
            _allShapes.Add(_linkRightShinShape);

            // 右足首
            _linkRightFoot = new Link()
            {
                LinkId       = 6,
                ParentLinkId = 5,
                Position     = new Vec4(0 / scale, 0 / scale, -46.5 / scale),
                Orientation  = Quaternion.CreateFromAngle(0, 0, 0),
            };
            var _linkRightFootShape = new Shape()
            {
                Type        = ShapeType.Pyramid,
                Scale       = new Vec4(16.5 / scale, 2 / scale, 2 / scale, 0 / scale),
                Orientation = Quaternion.CreateFromAngle(0, Math.PI / 2, -Math.PI / 2),
                Position    = new Vec4(0, 0, 0, 0),
            };
            _allLinkes.Add(_linkRightFoot);
            _allShapes.Add(_linkRightFootShape);

            #endregion

            // キネマティクスの初期化
            _kvm = new KinematicsViewModel(new ForwardKinematicsSolver());
            _kvm.Init(_allLinkes, _allShapes);

            // 描画領域に追加
            foreach (var s in _allShapes)
            {
                imuzDraw1.AddPyramidModel((float)s.Scale.x, (float)s.Scale.y, (float)s.Scale.z);
            }

            return;
        }
Beispiel #3
0
        private void Form1_Load(object sender, EventArgs e)
        {
            _estimator = new CompositePoseEstimator[numberOfLink];

            // フィルタ:速い動きに追従する設定です
            var fs = 1 / 0.01;
            var cutoff = 25.0;
            var tc = 0.4;
            for (int i = 0; i < numberOfLink; i++) {
                _estimator[i] = new CompositePoseEstimator();
                _estimator[i].SetFilterCoeffs(CompositePoseEstimator.FilterName.AccLowPassFilter, FilterDisignerBiquad.LowPass(fs, cutoff, 1.0));
                _estimator[i].SetFilterCoeffs(CompositePoseEstimator.FilterName.CompLowPassFilter, FilterDisignerBiquad.LowPass(fs, cutoff, 1.0));
                _estimator[i].SetFilterCoeffs(CompositePoseEstimator.FilterName.GyroLowPassFilter, FilterDisignerBiquad.LowPass(fs, 45.0, 1.0));
                _estimator[i].TimeConstantComposition = tc;
            }

            // 接続処理
            var dialog = new PortOpenDialog();
            if (dialog.ShowDialog() != System.Windows.Forms.DialogResult.OK) { return; }

            _port = CommunicationManager.CreateCommunicationPort(dialog.ConnectionNameType);
            // データ更新イベントの登録
            _port.MsgMeasurementReceived += new ReceivedMsgMeasurementHandler(_port_MsgMeasurementReceived);

            dialog.Dispose();
            _port.Open();

            #region 形状、位置設定
            // 骨盤
            _linkPelvis = new Link() {
                LinkId = 0,
                ParentLinkId = -1,
                Position = new Vec4(),
                Orientation = Quaternion.CreateFromAngle(0, 0, 0),
            };
            var _linkPelvisShape = new Shape() {
                Type = ShapeType.Pyramid,
                Scale = new Vec4(30.7 / scale, 3 / scale, 10 / scale, 0 / scale),
                Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0),
                Position = new Vec4(0, 0, 0, 0),
            };
            _allLinkes.Add(_linkPelvis);
            _allShapes.Add(_linkPelvisShape);

            // 左大腿
            _linkLeftThigh = new Link() {
                LinkId = 1,
                ParentLinkId = 0,
                Position = new Vec4(15.4 / scale, 0 / scale, 0 / scale),
                Orientation = Quaternion.CreateFromAngle(0, 0, 0),
            };
            var _linkLeftThighShape = new Shape() {
                Type = ShapeType.Pyramid,
                Scale = new Vec4(-38.1 / scale, 5 / scale, 3 / scale, 0 / scale),
                Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0),
                Position = new Vec4(0, 0, 0, 0),
            };
            _allLinkes.Add(_linkLeftThigh);
            _allShapes.Add(_linkLeftThighShape);

            // 左脛
            _linkLeftShin = new Link() {
                LinkId = 2,
                ParentLinkId = 1,
                Position = new Vec4(0 / scale, 0 / scale, -38.1 / scale),
                Orientation = Quaternion.CreateFromAngle(0, 0, 0),
            };
            var _linkLeftShinShape = new Shape() {
                Type = ShapeType.Pyramid,
                Scale = new Vec4(-46.5 / scale, 5 / scale, 3 / scale, 0 / scale),
                Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0),
                Position = new Vec4(0, 0, 0, 0),
            };
            _allLinkes.Add(_linkLeftShin);
            _allShapes.Add(_linkLeftShinShape);

            // 左足首
            _linkLeftFoot = new Link() {
                LinkId = 3,
                ParentLinkId = 2,
                Position = new Vec4(0 / scale, 0 / scale, -46.5 / scale),
                Orientation = Quaternion.CreateFromAngle(0, 0, 0),
            };
            var _linkLeftFootShape = new Shape() {
                Type = ShapeType.Pyramid,
                Scale = new Vec4(16.5 / scale, 2 / scale, 2 / scale, 0 / scale),
                Orientation = Quaternion.CreateFromAngle(0, Math.PI / 2, -Math.PI / 2),
                Position = new Vec4(0, 0, 0, 0),
            };
            _allLinkes.Add(_linkLeftFoot);
            _allShapes.Add(_linkLeftFootShape);

            // 右大腿
            _linkRightThigh = new Link() {
                LinkId = 4,
                ParentLinkId = 0,
                Position = new Vec4(-15.4 / scale, 0 / scale, 0 / scale),
                Orientation = Quaternion.CreateFromAngle(0, 0, 0),
            };
            var _linkRightThighShape = new Shape() {
                Type = ShapeType.Pyramid,
                Scale = new Vec4(-38.1 / scale, 5 / scale, 3 / scale, 0 / scale),
                Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0),
                Position = new Vec4(0, 0, 0, 0),
            };
            _allLinkes.Add(_linkRightThigh);
            _allShapes.Add(_linkRightThighShape);

            // 右脛
            _linkRightShin = new Link() {
                LinkId = 5,
                ParentLinkId = 4,
                Position = new Vec4(0 / scale, 0 / scale, -38.1 / scale),
                Orientation = Quaternion.CreateFromAngle(0, 0, 0),
            };
            var _linkRightShinShape = new Shape() {
                Type = ShapeType.Pyramid,
                Scale = new Vec4(-46.5 / scale, 5 / scale, 3 / scale, 0 / scale),
                Orientation = Quaternion.CreateFromAngle(0, -Math.PI / 2, 0),
                Position = new Vec4(0, 0, 0, 0),
            };
            _allLinkes.Add(_linkRightShin);
            _allShapes.Add(_linkRightShinShape);

            // 右足首
            _linkRightFoot = new Link() {
                LinkId = 6,
                ParentLinkId = 5,
                Position = new Vec4(0 / scale, 0 / scale, -46.5 / scale),
                Orientation = Quaternion.CreateFromAngle(0, 0, 0),
            };
            var _linkRightFootShape = new Shape() {
                Type = ShapeType.Pyramid,
                Scale = new Vec4(16.5 / scale, 2 / scale, 2 / scale, 0 / scale),
                Orientation = Quaternion.CreateFromAngle(0, Math.PI / 2, -Math.PI / 2),
                Position = new Vec4(0, 0, 0, 0),
            };
            _allLinkes.Add(_linkRightFoot);
            _allShapes.Add(_linkRightFootShape);

            #endregion

            // キネマティクスの初期化
            _kvm = new KinematicsViewModel(new ForwardKinematicsSolver());
            _kvm.Init(_allLinkes, _allShapes);

            // 描画領域に追加
            foreach (var s in _allShapes) { imuzDraw1.AddPyramidModel((float)s.Scale.x, (float)s.Scale.y, (float)s.Scale.z); }

            return;
        }
Beispiel #4
0
 public static void SetupModel(ImuzDraw draw, KinematicsViewModel model)
 {
     foreach (Shape s in model.Shape) {
         switch (s.Type) {
             case ShapeType.Pyramid:
                 draw.AddPyramidModel((float)s.Scale.x, (float)s.Scale.y, (float)s.Scale.z);
                 break;
             default:
                 continue;
         }
     }
 }