init: 导入项目到 meswork Gitea

This commit is contained in:
XingCheng3
2026-05-29 10:07:05 +08:00
commit ad9a4106fb
3582 changed files with 460312 additions and 0 deletions

View File

@@ -0,0 +1,618 @@
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
}
}