361 lines
12 KiB
C#
361 lines
12 KiB
C#
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
|
||
}
|
||
|
||
}
|
||
|