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;
}
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;
}
