public GloveModel()
{
gc = GloveController.GetSingleton(ModelType.HandOnly);
// var mLogger = Logger.GetInstance(mw.txt_log);
// gc.RegisterLogger(mLogger);
//if (!gc.IsConnected((int)handType)) //接入手套
//{
// gc.Connect(chuankou, 0); //连接手套和串口
//}
rhb = Rehabilitation.GetSingleton();
dh = DataWarehouse.GetSingleton();
sc = SkeletonCalculator.GetSingleton("");
handinformation = HandInf.GetSingleton();
OptimizedData = HandInf.GetSingleton();
fram = new SkeletonJson();
Mychuankou = "COM3";
//pullDataTimer = new Timer(500);
//pullDataTimer.Elapsed += pullDataTimer_Tick;
//pullDataTimer.Start();
}
public static HandInf GetSingleton()
{
if (handinf == null)
{
handinf = new HandInf();
}
return(handinf);
}
public void GetData()
{
try
{
//send right
var f_r = dh.GetFrameData(handType, Definition.MODEL_TYPE);
// public enum NodeType
//{
// Wrist,
// Upperarm,
// Index_0,
// Index_1,
// Middle_0,
// Middle_1,
// Ring_0,
// Ring_1,
// Little_0, // 0 for near one, 1 for far one
// Little_1,
// Thumb_0,
// Thumb_1,
// Forearm,
// Palm
//}
//for (int i = 0; i < Definition.SENSOR_COUNT; i++)
//{
// Console.WriteLine("{0} Node 'W is {1}", (NodeType)i, f_r.Nodes[i].X);
//}
//Console.WriteLine("{0} Node 'x is {1}", (NodeType)11, f_r.Nodes[11].X);
var s = sc.UpdateRaw(f_r);
fram = s.ToSkeletonJson();
skeletonJson = s.ToJson();
handinformation = SkeletonJsonToHandinf(fram);
//Console.WriteLine("the skeleton json is :");
//Console.WriteLine(skeletonJson);
//Console.WriteLine("the skeleton string is :");
//Console.WriteLine(s.ToString());
//Console.WriteLine(fram.Joints[(int)JointType.Hand].W);
//Console.WriteLine(s.ToString());\
}
catch (Exception e)
{
//Console.WriteLine(e);
return;
}
}
public string ToJson()
{
if (handinf != null)
{
return(JsonConvert.SerializeObject(handinf));
}
else
{
handinf = new HandInf();
return(JsonConvert.SerializeObject(handinf));
}
}
//public HandInf SkeletonJsonToHandinf(SkeletonJson fram)
//{
// var handinf = HandInf.GetSingleton();
// //order psi->theta->phi
// // order z->y->x
// //handinf.global_roll_x = (float)QuaternionConverter.Quat2Euler(fram.Joints[0].ToQuaternion()).X;
// //handinf.global_pitch_y = (float)QuaternionConverter.Quat2Euler(fram.Joints[0].ToQuaternion()).Y;
// //handinf.global_yaw_z = (float)QuaternionConverter.Quat2Euler(fram.Joints[0].ToQuaternion()).Z;
// handinf.global_roll_x = (float)QuaternionConverter.Quat2Euler(fram.Joints[0].ToQuaternion()).Y;
// handinf.global_pitch_y = (float)QuaternionConverter.Quat2Euler(fram.Joints[0].ToQuaternion()).X;
// handinf.global_yaw_z = (float)QuaternionConverter.Quat2Euler(fram.Joints[0].ToQuaternion()).Z;
// for (int i = 0;i<5;i++)
// {
// if(i==0)
// {
// //因为这个地方的传感器原来对应着大拇指，只有2和3才有值。
// handinf.fingers[i].Mcp_x = (float)QuaternionConverter.Quat2Euler(fram.Joints[i * 3 + 2].ToQuaternion()).Z - 180;
// handinf.fingers[i].Mcp_z = (float)QuaternionConverter.Quat2Euler(fram.Joints[i * 3 + 2].ToQuaternion()).Y > 300? (float)QuaternionConverter.Quat2Euler(fram.Joints[i * 3 + 2].ToQuaternion()).Y -360: (float)QuaternionConverter.Quat2Euler(fram.Joints[i * 3 + 2].ToQuaternion()).Y;
// handinf.fingers[i].Pip = 180 - (float)QuaternionConverter.Quat2Euler(fram.Joints[i * 3 + 3].ToQuaternion()).Z;
// handinf.fingers[i].Dip = 180 - (float)QuaternionConverter.Quat2Euler(fram.Joints[i * 3 + 3].ToQuaternion()).Z;
// if(handinf.fingers[i].Mcp_x <= 90 && handinf.fingers[i].Mcp_x >=0 )
// {
// ;
// }
// else
// {
// if(handinf.fingers[i].Mcp_x >90)
// {
// handinf.fingers[i].Mcp_x = 90;
// }
// else
// {
// handinf.fingers[i].Mcp_x = 0;
// }
// }
// if (handinf.fingers[i].Mcp_z <= 10 && handinf.fingers[i].Mcp_z >= -30)
// {
// ;
// }
// else
// {
// if (handinf.fingers[i].Mcp_z > 10)
// {
// handinf.fingers[i].Mcp_z = 10;
// }
// else
// {
// handinf.fingers[i].Mcp_z = -30;
// }
// }
// if (handinf.fingers[i].Pip <= 90 && handinf.fingers[i].Pip >= 0)
// {
// ;
// }
// else
// {
// if (handinf.fingers[i].Pip > 90)
// {
// handinf.fingers[i].Pip = 90;
// }
// else
// {
// handinf.fingers[i].Pip = 0;
// }
// }
// if (handinf.fingers[i].Dip <= 90 && handinf.fingers[i].Dip >= 0)
// {
// ;
// }
// else
// {
// if (handinf.fingers[i].Dip > 90)
// {
// handinf.fingers[i].Dip = 90;
// }
// else
// {
// handinf.fingers[i].Dip = 0;
// }
// }
// if(handinf.fingers[i].Mcp_x > 50)
// {
// handinf.fingers[i].Mcp_z = 0;
// }
// }
// else
// {
// handinf.fingers[i].Mcp_x = (float)QuaternionConverter.Quat2Euler(fram.Joints[i * 3 + 1].ToQuaternion()).Z - 180;
// handinf.fingers[i].Mcp_z = (float)QuaternionConverter.Quat2Euler(fram.Joints[i * 3 + 1].ToQuaternion()).Y >300 ? (float)QuaternionConverter.Quat2Euler(fram.Joints[i * 3 + 1].ToQuaternion()).Y - 360: (float)QuaternionConverter.Quat2Euler(fram.Joints[i * 3 + 1].ToQuaternion()).Y;
// handinf.fingers[i].Pip = 180 - (float)QuaternionConverter.Quat2Euler(fram.Joints[i * 3 + 2].ToQuaternion()).Z;
// handinf.fingers[i].Dip = 180 - (float)QuaternionConverter.Quat2Euler(fram.Joints[i * 3 + 3].ToQuaternion()).Z;
// if (handinf.fingers[i].Mcp_x <= 90 && handinf.fingers[i].Mcp_x >= 0)
// {
// ;
// }
// else
// {
// if (handinf.fingers[i].Mcp_x > 90)
// {
// handinf.fingers[i].Mcp_x = 90;
// }
// else
// {
// handinf.fingers[i].Mcp_x = 0;
// }
// }
// if(i == 2 )
// {
// if (handinf.fingers[i].Mcp_z <= 10 && handinf.fingers[i].Mcp_z >= -10)
// {
// ;
// }
// else
// {
// if (handinf.fingers[i].Mcp_z > 10)
// {
// handinf.fingers[i].Mcp_z = 10;
// }
// else
// {
// handinf.fingers[i].Mcp_z = -10;
// }
// }
// }
// else if(i == 3)
// {
// if (handinf.fingers[i].Mcp_z <= 30 && handinf.fingers[i].Mcp_z >= -10)
// {
// ;
// }
// else
// {
// if (handinf.fingers[i].Mcp_z > 30)
// {
// handinf.fingers[i].Mcp_z = 30;
// }
// else
// {
// handinf.fingers[i].Mcp_z = -10;
// }
// }
// }
// else
// {
// ;
// }
// if (handinf.fingers[i].Pip <= 90 && handinf.fingers[i].Pip >= 0)
// {
// ;
// }
// else
// {
// if (handinf.fingers[i].Pip > 90)
// {
// handinf.fingers[i].Pip = 90;
// }
// else
// {
// handinf.fingers[i].Pip = 0;
// }
// }
// if (handinf.fingers[i].Dip <= 90 && handinf.fingers[i].Dip >= 0)
// {
// ;
// }
// else
// {
// if (handinf.fingers[i].Dip > 90)
// {
// handinf.fingers[i].Dip = 90;
// }
// else
// {
// handinf.fingers[i].Dip = 0;
// }
// }
// if(i != 4)
// {
// if (handinf.fingers[i].Mcp_x > 50)
// {
// handinf.fingers[i].Mcp_z = 0;
// }
// }
// }
// }
// return handinf;
//}
public HandInf SkeletonJsonToHandinf(SkeletonJson fram)
{
var handinf = HandInf.GetSingleton();
//order psi->theta->phi
// order z->y->x
//handinf.global_roll_x = (float)QuaternionConverter.Quat2Euler(fram.Joints[0].ToQuaternion()).X;
//handinf.global_pitch_y = (float)QuaternionConverter.Quat2Euler(fram.Joints[0].ToQuaternion()).Y;
//handinf.global_yaw_z = (float)QuaternionConverter.Quat2Euler(fram.Joints[0].ToQuaternion()).Z;
//handinf.global_roll_x = (float)QtoE_glove_to_model(fram.Joints[0].ToQuaternion()).Z;
//handinf.global_pitch_y = - (float)QtoE_glove_to_model(fram.Joints[0].ToQuaternion()).X;
//handinf.global_yaw_z = - (float)QtoE_glove_to_model(fram.Joints[0].ToQuaternion()).Y;
Quaternion q = EtoQ(0, -90, 0);
Quaternion q_inverse = new Quaternion();
q_inverse.W = q.W;
q_inverse.X = -q.X;
q_inverse.Y = -q.Y;
q_inverse.Z = -q.Z;
handinf.global_roll_x = -(float)QtoE_glove_to_model(fram.Joints[0].ToQuaternion() * q_inverse).X;
handinf.global_pitch_y = -(float)QtoE_glove_to_model(fram.Joints[0].ToQuaternion() * q_inverse).Z;
handinf.global_yaw_z = -(float)QtoE_glove_to_model(fram.Joints[0].ToQuaternion() * q_inverse).Y;
for (int i = 0; i < 5; i++)
{
if (i == 0)
{
//因为这个地方的传感器原来对应着大拇指，只有2和3才有值。
Quaternion qy = new Quaternion(0, 1, 0, 0);
var bend = (float)QtoE(fram.Joints[i * 3 + 2].ToQuaternion() * qy).Z;
var strech = 360 - (float)QtoE_glove_to_model(fram.Joints[i * 3 + 2].ToQuaternion() * qy).Y;
var bend1 = (float)QtoE(fram.Joints[i * 3 + 3].ToQuaternion()).Z;
bend = RestrictAngle(bend, -10, 98);
bend1 = RestrictAngle(bend1, 0, 90);
strech = RestrictAngle(strech, -20, 10);
if (bend <= 30)
{
strech = strech;
}
else if (bend <= 50)
{
strech = RestrictAngle(strech, -10, 10);
}
else
{
strech = 0;
}
handinf.fingers[i].Mcp_x = bend;
handinf.fingers[i].Mcp_z = strech;
handinf.fingers[i].Pip = bend1;
handinf.fingers[i].Dip = bend1;
}
else
{
if (i == 4)
{
Quaternion q1 = fram.Joints[i * 3 + 1].ToQuaternion();
Quaternion q2 = new Quaternion();
q2.W = 0;
q2.X = 0;
q2.Y = 1;
q2.Z = 0;
Quaternion q3 = q1 * q2;
handinf.fingers[i].Mcp_x = (float)QtoE_glove_to_model(q3).Z;
handinf.fingers[i].Mcp_z = -(float)QtoE_glove_to_model(q3).Y;
handinf.fingers[i].Pip = RestrictAngle((float)QtoE(fram.Joints[i * 3 + 2].ToQuaternion()).Z, 0, 90);
handinf.fingers[i].Dip = -(float)QtoE_glove_to_model(q3).X;
}
else
{
Quaternion qy = new Quaternion(0, 1, 0, 0);
var bend = (float)QtoE(fram.Joints[i * 3 + 1].ToQuaternion() * qy).Z;
var strech = 360 - (float)QtoE_glove_to_model(fram.Joints[i * 3 + 1].ToQuaternion() * qy).Y;
var bend1 = (float)QtoE(fram.Joints[i * 3 + 2].ToQuaternion()).Z;
bend = RestrictAngle(bend, -10, 98);
bend1 = RestrictAngle(bend1, 0, 90);
if (i == 1)
{
strech = RestrictAngle(strech, -15, 15);
}
else if (i == 2)
{
strech = RestrictAngle(strech, -10, 10);
}
else
{
strech = RestrictAngle(strech, -20, 30);
}
if (bend <= 30)
{
strech = strech;
}
else if (bend <= 50)
{
strech = RestrictAngle(strech, -10, 10);
}
else
{
strech = 0;
}
handinf.fingers[i].Mcp_x = bend;
handinf.fingers[i].Mcp_z = strech;
handinf.fingers[i].Pip = bend1;
handinf.fingers[i].Dip = bend1;
}
}
}
return(handinf);
}
