meswork/01-Siyuan-MesProject
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

init: 导入项目到 meswork Gitea

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files

360
DataMirrorTools_Simulator/Foundation/Kinematics/Kinematics.Pose.cs Normal file
View File

@@ -0,0 +1,360 @@
using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
namespace KinematicsBase
{
[Serializable]
public class HomeOpen
{
public static string HOME = "HOME";
public static string OPEN = "OPEN";
public static string Position = "Position";
public static string Current = "Current";
}
[Serializable]
public class Pose
{
public static event Action<string, Pose> OnDataChange = null;
public Pose()
{
this.Id = Guid.NewGuid().ToString("D").ToUpper();
}
public Pose(Kinematics k)
{
this.Id = Guid.NewGuid().ToString("D").ToUpper();
JointPoseListCreate(k);
}
public Pose(string id, string name)
{
this.Id = id;
this.Name = name;
}
public string Id { get; set; }
public string Name { get; set; }
public string ModelId { get; set; }
/// <summary>
/// ture 独立运行
/// </summary>
public bool IsJointByself { get; set; }
public List<JointPose> JointPoseList { get; set; }
/// <summary>
/// 关节是否联动
/// </summary>
public bool JointGearing { get; set; }
/// <summary>
/// 时间优先时候的时间，单位毫秒
/// </summary>
public int Interval { get; set; }
/// <summary>
/// 时间优先或速度优先
/// </summary>
public MoveToPosePriorType m_MoveToPosePriorType { get; set; }
/// <summary>
/// string JointId double value
/// </summary>
public Dictionary<string, double> PoseInfo { get; set; } = new Dictionary<string, double>();
#region Method
public static void DataChange(string ModelId, Pose pose)
{
if (OnDataChange != null)
{
OnDataChange.Invoke(ModelId, pose);
}
}
private void JointPoseListCreate(Kinematics k)
{
this.JointPoseList = new List<JointPose>();
var JointListOrderByName = k.JointList.OrderBy(o => o.Name).ToList();
foreach (var item in JointListOrderByName)
{
this.JointPoseList.Add(new JointPose(item.Id, item.Name));
}
}
//public static void PoseListViewCreate(List<Pose> PoseViewList, List<Joint> joints)
//{
// Pose pose;
// double val=-1;
// pose = new Pose();
// pose.Name = "JointPosition";
// pose.Id = Guid.NewGuid().ToString("D").ToUpper();
// pose.Interval = 3000;
// //删除相同的JointList
// PoseListViewRemove(PoseViewList, joints);
// foreach (var item in joints)
// {
// 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;
// }
// pose.PoseInfo.Add(item.Id, val);
// }
// PoseViewList.Add(pose);
//}
//public static void PoseListViewRemove(List<Pose> PoseViewList, List<Joint> joints)
//{
// Pose pose=null;
// foreach(var joint in joints)
// {
// foreach (var item in PoseViewList)
// {
// if (item.PoseInfo.ContainsKey(joint.Id))
// {
// pose = item;
// break;
// }
// }
// }
// if(pose != null)
// {
// PoseViewList.Remove(pose);
// }
//}
//public static void PoseCurrentViewListSetValue(List<Pose> PoseList, string id,double val)
//{
// foreach(var poseItem in PoseList)
// {
// if (poseItem.PoseInfo.ContainsKey(id))
// {
// poseItem.PoseInfo[id] = val;
// break;
// }
// }
//}
public static void HomeAndOpenPositionCreate(Kinematics k)
{
Pose pose;
//HOME
pose = Pose.CreatePoseHome(k);
Pose.PoseListAddAndUpdateByName(k.PoseList, pose);
//OPEN
pose = Pose.CreatePoseOpen(k);
Pose.PoseListAddAndUpdateByName(k.PoseList, pose);
////Position
//pose = Pose.PosePositionCreator(k);
//Pose.PoseListAddAndUpdateByName( k.PoseList, pose);
}
public static Pose PosePositionCreator(Kinematics k)
{
Pose pose;
double val = -1;
pose = new Pose(k);
pose.Name = "Position";
pose.ModelId = k.ModelId;
pose.Interval = 3000;
pose.PoseInfo = new Dictionary<string, double>();
foreach (var item in k.JointList)
{
var joint = k.JointList.FirstOrDefault(t => t.Id == item.Id);
val = Joint.GetJointPosition(joint);
pose.PoseInfo.Add(item.Id, val);
}
return pose;
}
//public static Pose PoseCurrentCreator(List<Joint> joints)
//{
// Pose pose;
// double val = -1;
// pose = new Pose(k);
// pose.Name = "Current";
// pose.Id = Guid.NewGuid().ToString("D").ToUpper();
// pose.Interval = 3000;
// foreach (var item in joints)
// {
// 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;
// }
// pose.PoseInfo.Add(item.Id, val);
// }
// return pose;
//}
public static void PoseListAddAndUpdateByName(List<Pose> PoseList, Pose val)
{
var poseIndex = PoseList.FindIndex(t => t.Name == val.Name);
if (poseIndex > -1)
{
PoseList[poseIndex] = val;
}
else
{
PoseList.Add(val);
}
}
//public static void PoseListAddAndUpdate(List<Pose> PoseList, Pose val)
//{
// var obj = PoseList.FirstOrDefault(t => t.Id == val.Id);
// if (obj != null)
// {
// val.Id = obj.Id;
// PoseList.Remove(obj);
// PoseList.Add(val);
// }
// else
// {
// PoseList.Add(val);
// }
//}
//public static void PoseListRemoveById(List<Pose> PoseList, string id)
//{
// var obj = PoseList.FirstOrDefault(t => t.Id == id);
// if (obj != null)
// {
// PoseList.Remove(obj);
// }
//}
public static Pose PoseListFindById(List<Pose> PoseList, string id)
{
return PoseList.FirstOrDefault(t => t.Id == id);
}
//public static void PoseListAddAndUpdate(List<Pose> ObjList, List<Pose> objList)
//{
// List<Pose> objListBak = new List<Pose>(objList);
// foreach (var item in objList)
// {
// var pose = ObjList.FirstOrDefault(t => t.Id == item.Id);
// if (pose!=null)
// {
// Pose pValue = objListBak.FirstOrDefault(t => t.Id == item.Id);
// pValue.Id = pose.Id;
// ObjList.Remove(pose);
// ObjList.Add(pValue);
// }
// else
// {
// ObjList.Add(item);
// }
// }
//}
//public static void PoseListAddAndUpdateHomeAndPose(List<Joint> JointList, List<Pose> PostList)
//{
// Pose pose;
// pose = CreatePoseHome(JointList);
// PoseListAddAndUpdateByName(PostList, pose);
// pose = CreatePoseOpen(JointList);
// PoseListAddAndUpdateByName(PostList, pose);
//}
/// <summary>
/// 用 Joint最小值
/// </summary>
/// <param name="JointList"></param>
/// <returns></returns>
public static Pose CreatePoseHome(Kinematics k)
{
Pose pose;
pose = new Pose(k);
pose.Name = HomeOpen.HOME;
pose.ModelId = k.ModelId;
pose.Interval = 3000;
pose.PoseInfo.Clear();
foreach (Joint joint in k.JointList)
{
var list_Limit = new List<double>();
list_Limit.Add(Math.Abs(joint.JointLimit.LowLimit));
list_Limit.Add(Math.Abs(joint.JointLimit.HighLimit));
pose.PoseInfo.Add(joint.Id, list_Limit.Min());
}
return pose;
}
/// <summary>
/// 用 Joint最大值
/// </summary>
/// <param name="JointList"></param>
/// <returns></returns>
public static Pose CreatePoseOpen(Kinematics k)
{
Pose pose;
pose = new Pose(k);
pose.Name = HomeOpen.OPEN;
pose.ModelId = k.ModelId;
pose.Interval = 3000;
pose.PoseInfo.Clear();
foreach (Joint joint in k.JointList)
{
var list_Limit = new List<double>();
list_Limit.Add(Math.Abs(joint.JointLimit.LowLimit));
list_Limit.Add(Math.Abs(joint.JointLimit.HighLimit));
pose.PoseInfo.Add(joint.Id, list_Limit.Max());
}
return pose;
}
#endregion
}
}