619 lines
21 KiB
C#
619 lines
21 KiB
C#
using Newtonsoft.Json;
|
||
using Newtonsoft.Json.Linq;
|
||
using System;
|
||
using System.Collections;
|
||
using System.Collections.Generic;
|
||
using System.IO;
|
||
using System.Linq;
|
||
using System.Runtime.Serialization;
|
||
using System.Runtime.Serialization.Formatters.Binary;
|
||
using System.Xml.Linq;
|
||
|
||
namespace KinematicsBase
|
||
{
|
||
[Serializable]
|
||
public class Kinematics
|
||
{
|
||
#region Fields
|
||
|
||
public string Id { get; set; }
|
||
public string ModelId { get; set; }
|
||
|
||
public string ModelName { get; set; }
|
||
|
||
public string OpName { get; set; }
|
||
|
||
public DeviceType DeviceType { get; set; }
|
||
|
||
public PositionQuaternionWithID Base { get; set; }
|
||
|
||
public PositionQuaternionWithID Flange { get; set; }
|
||
|
||
public PositionQuaternionWithID Tool { get; set; }
|
||
|
||
public List<Link> LinkList { get; set; } = new List<Link>();
|
||
|
||
public List<Joint> JointList { get; set; } = new List<Joint>();
|
||
/// <summary>
|
||
/// 按父子级排序
|
||
/// </summary>
|
||
public List<Joint> JointListOrderBy { get; set; } = new List<Joint>();
|
||
|
||
|
||
/// <summary>
|
||
/// 创建Joint时,自动创建两个姿态。Joint的最小值作为HOME姿态，最大值作为姿态POSE
|
||
/// </summary>
|
||
public List<Pose> PoseList = new List<Pose>();
|
||
public List<Sensor> SensorList = new List<Sensor>();
|
||
|
||
|
||
#endregion
|
||
|
||
#region Constructor
|
||
public Kinematics()
|
||
{
|
||
|
||
|
||
}
|
||
|
||
|
||
|
||
#endregion
|
||
|
||
#region Link
|
||
|
||
/// <summary>
|
||
/// 根据Id删除Link
|
||
///
|
||
/// </summary>
|
||
/// <param name="id"></param>
|
||
public void RemoveLinkById(string id)
|
||
{
|
||
Link currentLink = null;
|
||
foreach (var link in LinkList)
|
||
{
|
||
if (link.Id == id)
|
||
{
|
||
currentLink = link;
|
||
break;
|
||
}
|
||
}
|
||
|
||
LinkList.Remove(currentLink);
|
||
}
|
||
|
||
|
||
|
||
/// <summary>
|
||
/// 根据Id（内部ID，非模型Id）获取Link name，和模型Id
|
||
/// </summary>
|
||
/// <param name="id"></param>
|
||
/// <returns></returns>
|
||
public string[] GetLinkNameModelIdById(string id)
|
||
{
|
||
string[] str = new[] { "Nan", "Nan" };
|
||
foreach (var link in LinkList)
|
||
{
|
||
if (link.Id == id)
|
||
{
|
||
str[0] = link.Name;
|
||
str[1] = link.LinkElements[0].Id;
|
||
return str;
|
||
}
|
||
}
|
||
|
||
return str;
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 根据ID获取Link
|
||
/// </summary>
|
||
/// <param name="linkList"></param>
|
||
/// <param name="id"></param>
|
||
/// <returns></returns>
|
||
public Link GetLinkById(string id)
|
||
{
|
||
foreach (var link in LinkList)
|
||
{
|
||
if (link.Id == id)
|
||
{
|
||
return link;
|
||
}
|
||
}
|
||
|
||
return null;
|
||
}
|
||
|
||
public List<string> GetLastLinkId()
|
||
{
|
||
var res = new List<string>();
|
||
|
||
//var lastLinkId = "";
|
||
//var lastLinkName = "";
|
||
|
||
//foreach (var link in LinkList)
|
||
//{
|
||
// foreach (var joint in JointList)
|
||
// {
|
||
// if (link.LinkElements[0].Id == joint.ParentLinkId)
|
||
// {
|
||
// continue;
|
||
// }
|
||
|
||
// lastLinkId = link.LinkElements[0].Id;
|
||
// lastLinkName = link.LinkElements[0].Name;
|
||
// }
|
||
//}
|
||
|
||
|
||
var link = LinkList.Find(o=>o.Name.Contains("_6"));
|
||
var lastLinkId = link.LinkElements[0].Id;
|
||
var lastLinkName = link.LinkElements[0].Name;
|
||
|
||
res.Add(lastLinkId);
|
||
res.Add(lastLinkName);
|
||
return res;
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
#endregion
|
||
#region Clone
|
||
public static List<T> Clone<T>(object List)
|
||
{
|
||
using (Stream objectStream = new MemoryStream())
|
||
{
|
||
IFormatter formatter = new BinaryFormatter();
|
||
formatter.Serialize(objectStream, List);
|
||
objectStream.Seek(0, SeekOrigin.Begin);
|
||
return formatter.Deserialize(objectStream) as List<T>;
|
||
}
|
||
}
|
||
#endregion
|
||
|
||
#region SensorSetup
|
||
public static void SensorSetup()
|
||
{
|
||
foreach (var k in PP.ReturnKinematicsList())
|
||
{
|
||
foreach (var sensor in k.SensorList)
|
||
{
|
||
sensor.Setup();
|
||
}
|
||
}
|
||
}
|
||
#endregion
|
||
#region Joint
|
||
public static void SetJointValueCurrent(string modelId, string id, double val)
|
||
{
|
||
// Kinematics k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId);
|
||
var k = PP.ModelCodeFindModelAttribute(modelId).KinematicsData;
|
||
k.JointList.FirstOrDefault(t => t.Id == id).ValueCurrent = val;
|
||
Joint.DataChange(k.ModelId, id, val);
|
||
}
|
||
|
||
//public static void SetJointValueCurrentPosition(string modelId, string id, double val,double valePosition)
|
||
//{
|
||
// Kinematics k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId);
|
||
// k.JointList.FirstOrDefault(t => t.Id == id).ValueCurrent = val+ valePosition;
|
||
// Joint.DataChange(k.ModelId, id, val);
|
||
//}
|
||
//public void SetJointValueCurrent(string id, double val)
|
||
//{
|
||
// //Kinematics k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId);
|
||
// this.JointList.FirstOrDefault(t => t.Id == id).ValueCurrent = val;
|
||
//}
|
||
public static void SetJointListValueCurrent(string modelId, Pose pose)
|
||
{
|
||
// Kinematics k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId);
|
||
var k = PP.ModelCodeFindModelAttribute(modelId).KinematicsData;
|
||
foreach (var item in k.JointList)
|
||
{
|
||
item.ValueCurrent = pose.PoseInfo[item.Id];
|
||
|
||
Joint.DataChange(k.ModelId, item.Id, Convert.ToSingle(item.ValueCurrent));
|
||
}
|
||
}
|
||
//public void SetJointListValueCurrent(Pose pose)
|
||
//{
|
||
// foreach (var item in this.JointList)
|
||
// {
|
||
// item.Value = pose.PoseInfo[item.Id];
|
||
// }
|
||
//}
|
||
|
||
public static string GoeModleIdByPoseId(string poseId)
|
||
{
|
||
foreach (var k in PP.ReturnKinematicsList())
|
||
{
|
||
var pose = k.PoseList.FirstOrDefault(t => t.Id == poseId);
|
||
if (pose != null)
|
||
{
|
||
return k.ModelId;
|
||
}
|
||
}
|
||
return null;
|
||
}
|
||
|
||
public static double GetJointValueCurrent(string modelId, string id)
|
||
{
|
||
//Kinematics k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId);
|
||
var k = PP.ModelCodeFindModelAttribute(modelId).KinematicsData;
|
||
return k.JointList.FirstOrDefault(t => t.Id == id).ValueCurrent;
|
||
}
|
||
public static Pose GetPoseByPoseId(string poseId)
|
||
{
|
||
List<Pose> PoseListView = new List<Pose>();
|
||
foreach (var item in PP.ReturnKinematicsList())
|
||
{
|
||
PoseListView.AddRange(item.PoseList);
|
||
}
|
||
return PoseListView.FirstOrDefault(t => t.Id == poseId);
|
||
}
|
||
|
||
|
||
public static List<Joint> GetJointListView()
|
||
{
|
||
List<Joint> JointListView = new List<Joint>();
|
||
foreach (var item in PP.ReturnKinematicsList())
|
||
{
|
||
JointListView.AddRange(item.JointList);
|
||
}
|
||
return JointListView;
|
||
}
|
||
public static List<Sensor> GetSensorListView()
|
||
{
|
||
List<Sensor> SensorListView = new List<Sensor>();
|
||
foreach (var item in PP.ReturnKinematicsList())
|
||
{
|
||
SensorListView.AddRange(item.SensorList);
|
||
}
|
||
return SensorListView;
|
||
}
|
||
//SensorListView
|
||
|
||
//JointListView
|
||
public static Pose GetPoseByModeIdAndPoseId(string modelId, string poseId)
|
||
{
|
||
// return DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId).PoseList.FirstOrDefault(t => t.Id == poseId);
|
||
return PP.ModelCodeFindModelAttribute(modelId).KinematicsData.PoseList.FirstOrDefault(t => t.Id == poseId);
|
||
}
|
||
public static string GetModelNameByModelId(string modelId)
|
||
{
|
||
// Kinematics k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId);
|
||
var k = PP.ModelCodeFindModelAttribute(modelId).KinematicsData;
|
||
return k.ModelName;
|
||
}
|
||
public static List<Joint> GetJointListByModelId(string modelId)
|
||
{
|
||
// return DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId).JointList;
|
||
return PP.ModelCodeFindModelAttribute(modelId).KinematicsData.JointList;
|
||
}
|
||
public static Joint GetJointByModelIdAndJointId(string modelId, string jointId)
|
||
{
|
||
// return DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId).JointList.FirstOrDefault(t => t.Id == jointId);
|
||
return PP.ModelCodeFindModelAttribute(modelId).KinematicsData.JointList.FirstOrDefault(t => t.Id == jointId);
|
||
}
|
||
public static Kinematics GetKinematicsByModelId(string modelId)
|
||
{
|
||
// return DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId);
|
||
return PP.ModelCodeFindModelAttribute(modelId).KinematicsData;
|
||
|
||
}
|
||
//Kinematics.SetJointValueCurrent(modelId, jId, valuCurrent);
|
||
/// <summary>
|
||
/// 根据Id删除joint
|
||
/// </summary>
|
||
/// <param name="id"></param>
|
||
public void RemeveJointById(string id)
|
||
{
|
||
Joint currentJoint = null;
|
||
foreach (var joint in JointList)
|
||
{
|
||
if (joint.Id == id)
|
||
{
|
||
currentJoint = joint;
|
||
break;
|
||
}
|
||
}
|
||
|
||
JointList.Remove(currentJoint);
|
||
|
||
}
|
||
|
||
///// <summary>
|
||
///// 根据ID获取Joint
|
||
///// </summary>
|
||
///// <param name="jointList"></param>
|
||
///// <param name="id"></param>
|
||
///// <returns></returns>
|
||
//public Joint GetJointById(string id)
|
||
//{
|
||
// foreach (var joint in JointList)
|
||
// {
|
||
// if (joint.Id == id)
|
||
// {
|
||
// return joint;
|
||
// }
|
||
// }
|
||
|
||
// return null;
|
||
//}
|
||
|
||
|
||
|
||
//topId = 0;
|
||
// var list = GetSonNode(topId, nodeList);
|
||
|
||
// foreach (var item in list)
|
||
// {
|
||
// Console.WriteLine("id = "+item.Id+ "/nParentId="+item.ParentId+ "/nMame = " +item.Name);
|
||
// }
|
||
/// <summary>
|
||
/// Joint 按父节点到子节点递归排序
|
||
/// </summary>
|
||
/// <param name="id"></param>
|
||
/// <param name="nodeList"></param>
|
||
/// <returns></returns>
|
||
private List<Joint> GetSonNode(string ChildLinkId, List<Joint> nodeList)
|
||
{
|
||
|
||
|
||
var query = nodeList.Where(c => c.ParentLinkId == ChildLinkId);
|
||
|
||
return query.ToList().Concat(query.ToList().SelectMany(t => GetSonNode(t.ChildLinkId, nodeList))).ToList();
|
||
}
|
||
private string GetParents(string ChildLinkId, List<Joint> list)
|
||
{
|
||
List<Joint> result = list.Where(x => x.ChildLinkId == ChildLinkId).ToList();
|
||
if (result.Count > 0 && result[0].ParentLinkId != "" && result[0].ParentLinkId.ToString() != "")
|
||
return GetParents(result[0].ParentLinkId, list);
|
||
else
|
||
return ChildLinkId;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Joint 按父节点到子节点递归排序
|
||
/// </summary>
|
||
/// <returns></returns>
|
||
public List<Joint> GetSonNode()
|
||
{
|
||
List<Joint> nodeList = JointList;
|
||
if (nodeList.Count < 1) return null;
|
||
string ChildLinkId_init = nodeList[0].ChildLinkId;
|
||
string ChildLinkId = GetParents(ChildLinkId_init, nodeList);
|
||
List<Joint> result;
|
||
result = GetSonNode(ChildLinkId, nodeList);
|
||
|
||
return result;
|
||
}
|
||
/// <summary>
|
||
/// 关节联动时
|
||
/// 计算每个关节的值
|
||
/// </summary>
|
||
/// <param name="nodeList"></param>
|
||
/// <param name="val"></param>
|
||
public string JointListValueCurrent(double val)
|
||
{
|
||
JointListOrderBy = GetSonNode();
|
||
|
||
List<Joint> nodeList;
|
||
nodeList = JointListOrderBy;
|
||
JArray jarrayObj = new JArray();
|
||
|
||
Dictionary<string, double> dicJointListValueCurrent = new Dictionary<string, double>();
|
||
|
||
dicJointListValueCurrent.Add(nodeList[0].ParentLinkId, val);
|
||
for (int i = 0; i < nodeList.Count; i++)
|
||
{
|
||
nodeList[i].ValueCurrent = dicJointListValueCurrent[nodeList[i].ParentLinkId] * nodeList[i].ValueFactor;
|
||
if (!dicJointListValueCurrent.ContainsKey(nodeList[i].ChildLinkId))
|
||
{
|
||
dicJointListValueCurrent.Add(nodeList[i].ChildLinkId, nodeList[i].ValueCurrent);
|
||
}
|
||
}
|
||
foreach (var joint in nodeList)
|
||
{
|
||
var jObj = new JObject();
|
||
jObj["jId"] = joint.Id;
|
||
jObj["jValue"] = joint.ValueCurrent + Joint.GetJointPosition(joint); ;
|
||
jarrayObj.Add(jObj);
|
||
|
||
SetJointValueCurrent(this.ModelId, joint.Id, joint.ValueCurrent);
|
||
//Joint.DataChange(this.ModelId, joint.Id, joint.ValueCurrent);
|
||
|
||
}
|
||
var jsonData = new JObject(
|
||
new JProperty("ModelId", this.ModelId),
|
||
new JProperty("MotionType", "3"),
|
||
new JProperty("MotionInfo", jarrayObj)
|
||
);
|
||
var jsonStr = jsonData.ToString().Replace("\n", "").Replace("\r", "").Replace(" ", "");
|
||
string message = "|SendMotionInfo|" + jsonStr;
|
||
return message;
|
||
}
|
||
public List<C_ObjStates> JointListValueCurrentNew(double val)
|
||
{
|
||
List<C_ObjStates> ObjStatesList = new List<C_ObjStates>();
|
||
List<C_ObjStates> objStates = new List<C_ObjStates>();
|
||
JointListOrderBy = GetSonNode();
|
||
|
||
List<Joint> nodeList;
|
||
nodeList = JointListOrderBy;
|
||
JArray jarrayObj = new JArray();
|
||
|
||
Dictionary<string, double> dicJointListValueCurrent = new Dictionary<string, double>();
|
||
|
||
dicJointListValueCurrent.Add(nodeList[0].ParentLinkId, val);
|
||
for (int i = 0; i < nodeList.Count; i++)
|
||
{
|
||
nodeList[i].ValueCurrent = dicJointListValueCurrent[nodeList[i].ParentLinkId] * nodeList[i].ValueFactor;
|
||
if (!dicJointListValueCurrent.ContainsKey(nodeList[i].ChildLinkId))
|
||
{
|
||
dicJointListValueCurrent.Add(nodeList[i].ChildLinkId, nodeList[i].ValueCurrent);
|
||
}
|
||
}
|
||
foreach (var joint in nodeList)
|
||
{
|
||
var jObj = new JObject();
|
||
jObj["jId"] = joint.Id;
|
||
jObj["jValue"] = joint.ValueCurrent + Joint.GetJointPosition(joint); ;
|
||
jarrayObj.Add(jObj);
|
||
|
||
SetJointValueCurrent(this.ModelId, joint.Id, joint.ValueCurrent);
|
||
|
||
objStates = new OPERATION_FramesList().GetObjStates(this.ModelId, joint.Id, joint.ValueCurrent);
|
||
foreach (var item in objStates)
|
||
{
|
||
ObjStatesList.Add(item);
|
||
}
|
||
//Joint.DataChange(this.ModelId, joint.Id, joint.ValueCurrent);
|
||
|
||
}
|
||
|
||
return ObjStatesList;
|
||
}
|
||
|
||
|
||
#endregion
|
||
|
||
#region KinematicsList
|
||
|
||
/// <summary>
|
||
/// 根据设备代码判断设备是否存在
|
||
/// </summary>
|
||
/// <param name="modelId"></param>
|
||
/// <returns></returns>
|
||
public static Kinematics KinematicsListFindByModelId(string modelId)
|
||
{
|
||
// var k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId);
|
||
var k = PP.ModelCodeFindModelAttribute(modelId).KinematicsData;
|
||
return k;
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 设备存在，更新；不存在增加
|
||
/// </summary>
|
||
/// <param name="k"></param>
|
||
/// <returns></returns>
|
||
public static void KinematicsListAddAndUpDate(Kinematics k)
|
||
{
|
||
// var objK = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == k.ModelId);
|
||
var objK = PP.ModelCodeFindModelAttribute(k.ModelId).KinematicsData;
|
||
if (objK == null)
|
||
{
|
||
// DataBaseJson.dataBaseJson.KinematicsList.Add(k);
|
||
foreach (var item in k.SensorList)
|
||
{
|
||
|
||
switch (item.SensorType)
|
||
{
|
||
case SensorType.IsRun:
|
||
Joint.OnDataChangeJointIsRun -= item.Joint_OnDataChangeJointIsRun;
|
||
Joint.OnDataChangeJointIsRun += item.Joint_OnDataChangeJointIsRun;
|
||
break;
|
||
default:
|
||
Joint.OnDataChange -= item.Joint_OnDataChange;
|
||
Joint.OnDataChange += item.Joint_OnDataChange;
|
||
break;
|
||
}
|
||
|
||
}
|
||
}
|
||
else
|
||
{
|
||
foreach (var item in k.SensorList)
|
||
{
|
||
|
||
switch (item.SensorType)
|
||
{
|
||
case SensorType.IsRun:
|
||
Joint.OnDataChangeJointIsRun -= item.Joint_OnDataChangeJointIsRun;
|
||
Joint.OnDataChangeJointIsRun += item.Joint_OnDataChangeJointIsRun;
|
||
break;
|
||
default:
|
||
Joint.OnDataChange -= item.Joint_OnDataChange;
|
||
Joint.OnDataChange += item.Joint_OnDataChange;
|
||
break;
|
||
}
|
||
|
||
}
|
||
|
||
objK = k;
|
||
}
|
||
|
||
// Event.KinematicsDataChange(k);
|
||
}
|
||
|
||
|
||
|
||
private double GetJointPosition(Kinematics k, string jointId)
|
||
{
|
||
double val = -1;
|
||
|
||
var item = k.JointList.FirstOrDefault(t => t.Id == jointId);
|
||
switch (item.JointType)
|
||
{
|
||
case JointType.Prismatic:
|
||
switch (item.Axis.AxisFlag)
|
||
{
|
||
case AxisFlag.X:
|
||
val = Convert.ToSingle(item.Axis.Loaction.Position.X);
|
||
break;
|
||
case AxisFlag.Y:
|
||
val = Convert.ToSingle(item.Axis.Loaction.Position.Y);
|
||
break;
|
||
case AxisFlag.Z:
|
||
val = Convert.ToSingle(item.Axis.Loaction.Position.Z);
|
||
break;
|
||
}
|
||
break;
|
||
//一、绕定轴X-Y-Z旋转（RPY角）
|
||
//二、绕动轴Z-Y-X旋转（Euler角）
|
||
case JointType.Revolute:
|
||
switch (item.Axis.AxisFlag)
|
||
{
|
||
case AxisFlag.X:
|
||
val = Convert.ToSingle(item.Axis.Loaction.FixedAngle.R);
|
||
break;
|
||
case AxisFlag.Y:
|
||
val = Convert.ToSingle(item.Axis.Loaction.FixedAngle.P);
|
||
break;
|
||
case AxisFlag.Z:
|
||
val = Convert.ToSingle(item.Axis.Loaction.FixedAngle.Y);
|
||
break;
|
||
}
|
||
break;
|
||
}
|
||
return val;
|
||
}
|
||
|
||
///// <summary>
|
||
///// 根据设备代码删除设备信息
|
||
///// </summary>
|
||
///// <param name="modelId"></param>
|
||
///// <returns></returns>
|
||
//public static bool KinematicsListRemoveByModelId(string modelId)
|
||
//{
|
||
// var k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == modelId);
|
||
// if (k != null)
|
||
// {
|
||
// DataBaseJson.dataBaseJson.KinematicsList.Remove(k);
|
||
// return true;
|
||
// }
|
||
// return false;
|
||
//}
|
||
|
||
|
||
#endregion
|
||
|
||
|
||
|
||
}
|
||
}
|