init: 导入项目到 meswork Gitea
This commit is contained in:
XingCheng3
@@ -0,0 +1,523 @@
|
||||
using System;
|
||||
using System.Collections;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
|
||||
namespace KinematicsBase
|
||||
{
|
||||
#region BaseClass
|
||||
|
||||
[Serializable]
|
||||
public enum UnitType
|
||||
{
|
||||
mm,
|
||||
cm,
|
||||
m
|
||||
}
|
||||
[Serializable]
|
||||
public enum LimitType
|
||||
{
|
||||
Constant,
|
||||
|
||||
NoLimits
|
||||
//,Variable
|
||||
}
|
||||
|
||||
public enum JointType
|
||||
{
|
||||
/// <summary>
|
||||
/// 旋转
|
||||
/// </summary>
|
||||
Revolute,
|
||||
/// <summary>
|
||||
/// 滑移
|
||||
/// </summary>
|
||||
Prismatic
|
||||
}
|
||||
[Serializable]
|
||||
public class JointMaxValues
|
||||
{
|
||||
public JointMaxValues()
|
||||
{
|
||||
}
|
||||
|
||||
public JointMaxValues(double speed, double acceleration)
|
||||
{
|
||||
Speed = speed;
|
||||
Acceleration = acceleration;
|
||||
}
|
||||
public double SpeedFromJointLimit(JointLimit jointLimit)
|
||||
{
|
||||
if (jointLimit.Interval <= 0.000000001)
|
||||
{
|
||||
Speed = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
Speed = (jointLimit.HighLimit - jointLimit.LowLimit) / (jointLimit.Interval / 1000f);
|
||||
}
|
||||
return Speed;
|
||||
}
|
||||
|
||||
public double Speed { get; set; }
|
||||
|
||||
public double Acceleration { get; set; }
|
||||
}
|
||||
[Serializable]
|
||||
public class JointLimit
|
||||
{
|
||||
public JointLimit()
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建JointLimit类构造方法
|
||||
/// </summary>
|
||||
/// <param name="limitType">LimitType. </param>
|
||||
/// <param name="lowLimit">最小极限 double </param>
|
||||
/// <param name="highLimit">最大极限 double</param>
|
||||
public JointLimit(LimitType limitType, double lowLimit, double highLimit)
|
||||
{
|
||||
this.LimitType = limitType;
|
||||
this.LowLimit = lowLimit;
|
||||
this.HighLimit = highLimit;
|
||||
}
|
||||
public JointLimit(LimitType limitType, double lowLimit, double highLimit, int interval)
|
||||
{
|
||||
this.LimitType = limitType;
|
||||
this.LowLimit = lowLimit;
|
||||
this.HighLimit = highLimit;
|
||||
this.Interval = interval;
|
||||
|
||||
}
|
||||
|
||||
public LimitType LimitType { get; set; }
|
||||
|
||||
public double LowLimit { get; set; }
|
||||
|
||||
public double HighLimit { get; set; }
|
||||
/// <summary>
|
||||
/// 时间优先时候的时间，单位毫秒
|
||||
/// </summary>
|
||||
public int Interval;
|
||||
}
|
||||
|
||||
[Serializable]
|
||||
public class Joint
|
||||
{
|
||||
/// <summary>
|
||||
/// ModeId JointId value
|
||||
/// </summary>
|
||||
public static event Action<string, string, object> OnDataChange = null;
|
||||
public static event Action<string, string, bool> OnDataChangeJointIsRun = null;
|
||||
//public static event Action<Joint> UpdateJoint;
|
||||
|
||||
public static void DataChange(string modeId, string id, object val)
|
||||
{
|
||||
if (OnDataChange != null)
|
||||
{
|
||||
OnDataChange.Invoke(modeId, id, val);
|
||||
}
|
||||
}
|
||||
|
||||
public static void DataChangeJointIsRun(string modeId, string id, bool val)
|
||||
{
|
||||
if (OnDataChangeJointIsRun != null)
|
||||
{
|
||||
OnDataChangeJointIsRun.Invoke(modeId, id, val);
|
||||
}
|
||||
}
|
||||
public Joint()
|
||||
{
|
||||
GraphPosition = new System.Drawing.Point[2];
|
||||
}
|
||||
|
||||
|
||||
public Joint(string id, string name, bool isDriving = true)
|
||||
{
|
||||
this.Id = id;
|
||||
this.Name = name;
|
||||
this.IsDriving = isDriving;
|
||||
//this.Valuefactor = 1;
|
||||
}
|
||||
public System.Drawing.Point[] GraphPosition { set; get; }
|
||||
|
||||
public string Id { get; set; }
|
||||
|
||||
public string Name { get; set; }
|
||||
public string ModelId { get; set; }
|
||||
/// <summary>
|
||||
/// 驱动轴
|
||||
/// 联动时的主运动
|
||||
/// </summary>
|
||||
public bool IsDriving { get; set; }
|
||||
|
||||
public string ParentLinkId { get; set; }
|
||||
|
||||
public string ChildLinkId { get; set; }
|
||||
/// <summary>
|
||||
/// 父模型代码
|
||||
/// </summary>
|
||||
public string PId { get; set; }
|
||||
/// <summary>
|
||||
/// 子模型代码
|
||||
/// </summary>
|
||||
public string CId { get; set; }
|
||||
|
||||
|
||||
public Axis Axis { get; set; }
|
||||
|
||||
public JointType JointType { get; set; }
|
||||
|
||||
public bool LockJoint { get; set; }
|
||||
|
||||
public JointLimit JointLimit { get; set; }
|
||||
|
||||
public JointMaxValues JointMaxValue { get; set; }
|
||||
/// <summary>
|
||||
/// 模型初始值
|
||||
/// </summary>
|
||||
public double Value { get; set; }
|
||||
|
||||
public bool FindJointById(Joint joint)
|
||||
{
|
||||
return Id == joint.Id;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 输入参数
|
||||
/// </summary>
|
||||
public double ValueInput { get; set; }
|
||||
/// <summary>
|
||||
/// 倍率 1
|
||||
/// </summary>
|
||||
public double ValueFactor { get; set; } = 1;
|
||||
/// <summary>
|
||||
/// 模型当前值
|
||||
/// </summary>
|
||||
public double ValueCurrent { get; set; }
|
||||
public string Script;
|
||||
public bool IsScript;
|
||||
|
||||
public bool IsOnlyHomePose;
|
||||
|
||||
|
||||
public static double GetJointPosition(Joint joint)
|
||||
{
|
||||
double val = -1;
|
||||
switch (joint.JointType)
|
||||
{
|
||||
case JointType.Prismatic:
|
||||
switch (joint.Axis.AxisFlag)
|
||||
{
|
||||
case AxisFlag.X:
|
||||
val = Convert.ToSingle(joint.Axis.Loaction.Position.X);
|
||||
break;
|
||||
case AxisFlag.Y:
|
||||
val = Convert.ToSingle(joint.Axis.Loaction.Position.Y);
|
||||
break;
|
||||
case AxisFlag.Z:
|
||||
val = Convert.ToSingle(joint.Axis.Loaction.Position.Z);
|
||||
break;
|
||||
}
|
||||
break;
|
||||
//一、绕定轴X-Y-Z旋转（RPY角）
|
||||
//二、绕动轴Z-Y-X旋转（Euler角）
|
||||
case JointType.Revolute:
|
||||
switch (joint.Axis.AxisFlag)
|
||||
{
|
||||
case AxisFlag.X:
|
||||
val = Convert.ToSingle(joint.Axis.Loaction.FixedAngle.R);
|
||||
break;
|
||||
case AxisFlag.Y:
|
||||
val = Convert.ToSingle(joint.Axis.Loaction.FixedAngle.P);
|
||||
break;
|
||||
case AxisFlag.Z:
|
||||
val = Convert.ToSingle(joint.Axis.Loaction.FixedAngle.Y);
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
return val;
|
||||
}
|
||||
/// <summary>
|
||||
/// 计算位姿变换
|
||||
/// </summary>
|
||||
/// <param name="initCoordinate">初始坐标doule[7]</param>
|
||||
/// <param name="along">沿着某个轴转，X/Y/Z/RX/RY/RZ</param>
|
||||
/// <param name="step">移动步长，转动单位弧度，移动单位与输入一致</param>
|
||||
/// <returns>目标坐标doule[7]</returns>
|
||||
public PositionQuaternion TransformCoordinates(Joint joint, double step)
|
||||
{
|
||||
string along = GetJointAlong(joint);
|
||||
double[] initCoordinate = new double[7];
|
||||
initCoordinate[0] = joint.Axis.LoactionQuaternion.Position.X;
|
||||
initCoordinate[1] = joint.Axis.LoactionQuaternion.Position.Y;
|
||||
initCoordinate[2] = joint.Axis.LoactionQuaternion.Position.Z;
|
||||
initCoordinate[3] = joint.Axis.LoactionQuaternion.Quaternion.X;
|
||||
initCoordinate[4] = joint.Axis.LoactionQuaternion.Quaternion.Y;
|
||||
initCoordinate[5] = joint.Axis.LoactionQuaternion.Quaternion.Z;
|
||||
initCoordinate[6] = joint.Axis.LoactionQuaternion.Quaternion.W;
|
||||
|
||||
|
||||
return TransformCoordinates(initCoordinate, along, step, joint.JointType);
|
||||
|
||||
|
||||
}
|
||||
|
||||
public static string GetJointAlong(Joint joint)
|
||||
{
|
||||
string along = "X";
|
||||
switch (joint.JointType)
|
||||
{
|
||||
case JointType.Prismatic:
|
||||
switch (joint.Axis.AxisFlag)
|
||||
{
|
||||
case AxisFlag.X:
|
||||
along = "X";
|
||||
break;
|
||||
case AxisFlag.Y:
|
||||
along = "Y";
|
||||
break;
|
||||
case AxisFlag.Z:
|
||||
along = "Z";
|
||||
break;
|
||||
}
|
||||
break;
|
||||
//一、绕定轴X-Y-Z旋转（RPY角）
|
||||
//二、绕动轴Z-Y-X旋转（Euler角）
|
||||
case JointType.Revolute:
|
||||
switch (joint.Axis.AxisFlag)
|
||||
{
|
||||
case AxisFlag.X:
|
||||
along = "RX";
|
||||
break;
|
||||
case AxisFlag.Y:
|
||||
along = "RY";
|
||||
break;
|
||||
case AxisFlag.Z:
|
||||
along = "RZ";
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
return along;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算位姿变换
|
||||
/// </summary>
|
||||
/// <param name="initCoordinate">初始坐标doule[7]</param>
|
||||
/// <param name="along">沿着某个轴转，X/Y/Z/RX/RY/RZ</param>
|
||||
/// <param name="step">移动步长，转动单位弧度，移动单位与输入一致</param>
|
||||
/// <returns>目标坐标doule[7]</returns>
|
||||
static public PositionQuaternion TransformCoordinates(double[] initCoordinate, string along, double step, JointType jointType)
|
||||
{
|
||||
PositionQuaternion PoseLoactionQuaternion = new PositionQuaternion();
|
||||
// 存储转换结果
|
||||
//double[] res = new double[7];
|
||||
if (jointType == JointType.Revolute)
|
||||
{
|
||||
step = step * Math.PI / 180;
|
||||
}
|
||||
var stepFloat = Convert.ToSingle(step);
|
||||
|
||||
// 初始姿态
|
||||
var initPos = new System.Numerics.Vector3(Convert.ToSingle(initCoordinate[0]), Convert.ToSingle(initCoordinate[1]), Convert.ToSingle(initCoordinate[2]));
|
||||
var initQua = new System.Numerics.Quaternion(Convert.ToSingle(initCoordinate[3]), Convert.ToSingle(initCoordinate[4]), Convert.ToSingle(initCoordinate[5]), Convert.ToSingle(initCoordinate[6]));
|
||||
var resPos = new System.Numerics.Vector3();
|
||||
var resQua = new System.Numerics.Quaternion();
|
||||
resPos = initPos;
|
||||
|
||||
// 转动或移动
|
||||
switch (along.ToUpper())
|
||||
{
|
||||
case "X":
|
||||
{
|
||||
resPos.X += stepFloat;
|
||||
resQua = initQua;
|
||||
break;
|
||||
}
|
||||
case "Y":
|
||||
{
|
||||
resPos.Y += stepFloat;
|
||||
resQua = initQua;
|
||||
break;
|
||||
}
|
||||
case "Z":
|
||||
{
|
||||
resPos.Z += stepFloat;
|
||||
resQua = initQua;
|
||||
break;
|
||||
}
|
||||
case "RX":
|
||||
{
|
||||
resPos = initPos;
|
||||
var quaTrans = System.Numerics.Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), stepFloat);
|
||||
resQua = System.Numerics.Quaternion.Multiply(initQua, quaTrans);
|
||||
break;
|
||||
}
|
||||
case "RY":
|
||||
{
|
||||
resPos = initPos;
|
||||
var quaTrans = System.Numerics.Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), stepFloat);
|
||||
resQua = System.Numerics.Quaternion.Multiply(initQua, quaTrans);
|
||||
break;
|
||||
}
|
||||
case "RZ":
|
||||
{
|
||||
resPos = initPos;
|
||||
var quaTrans = System.Numerics.Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), stepFloat);
|
||||
resQua = System.Numerics.Quaternion.Multiply(initQua, quaTrans);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
//res[0] = resPos.X/1000;
|
||||
//res[1] = resPos.Y/1000;
|
||||
//res[2] = resPos.Z/1000;
|
||||
//res[3] = resQua.X;
|
||||
//res[4] = resQua.Y;
|
||||
//res[5] = resQua.Z;
|
||||
//res[6] = resQua.W;
|
||||
PoseLoactionQuaternion.Position.X = resPos.X / 1000;
|
||||
PoseLoactionQuaternion.Position.Y = resPos.Y / 1000;
|
||||
PoseLoactionQuaternion.Position.Z = resPos.Z / 1000;
|
||||
PoseLoactionQuaternion.Quaternion.X = resQua.X;
|
||||
PoseLoactionQuaternion.Quaternion.Y = resQua.Y;
|
||||
PoseLoactionQuaternion.Quaternion.Z = resQua.Z;
|
||||
PoseLoactionQuaternion.Quaternion.W = resQua.W;
|
||||
return PoseLoactionQuaternion;
|
||||
}
|
||||
|
||||
#region Method
|
||||
//public static void JointListRemove(List<Joint> JointList,List<Joint>JointListOld)
|
||||
//{
|
||||
// if (JointListOld == null) return;
|
||||
// foreach(var item in JointListOld)
|
||||
// {
|
||||
// int index = JointList.FindIndex(t => t.Id == item.Id);
|
||||
// if (index >= 0)
|
||||
// {
|
||||
// JointList.RemoveAt(index);
|
||||
// }
|
||||
// }
|
||||
//}
|
||||
///// <summary>
|
||||
///// 将设备JointList加入系统列表
|
||||
///// </summary>
|
||||
///// <param name="JointList"></param>
|
||||
///// <param name="jointList"></param>
|
||||
//public static void JointListViewAddAndUpdate(List<Joint> JointList, List<Joint> jointList)
|
||||
//{
|
||||
// int index;
|
||||
// foreach (var item in jointList)
|
||||
// {
|
||||
// index = JointList.FindIndex(t => t.Id == item.Id);
|
||||
// if (index > -1)
|
||||
// {
|
||||
// JointList.RemoveAt(index);
|
||||
// item.ValueCurrent = item.Value;
|
||||
// JointList.Add(item);
|
||||
// Joint.DataChange(item.Id, item.Value);
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// item.ValueCurrent = item.Value;
|
||||
// JointList.Add(item);
|
||||
// Joint.DataChange(item.Id, item.Value);
|
||||
// }
|
||||
// }
|
||||
//}
|
||||
|
||||
|
||||
#endregion
|
||||
///// <summary>
|
||||
///// 当前实际位置
|
||||
///// </summary>
|
||||
//public PositionFixedAngle PositionCurrent { get; set; }
|
||||
///// <summary>
|
||||
///// 根据输入值和倍率计算模型当前位置
|
||||
///// </summary>
|
||||
///// <param name="ValueInput"></param>
|
||||
///// <returns></returns>
|
||||
//public PositionFixedAngle GetPositionCurrent(double ValueInput)
|
||||
//{
|
||||
|
||||
// Value = Value * ValueFactor;
|
||||
// this.ValueInput = ValueInput;
|
||||
// PositionCurrent.Position.X = Axis.Loaction.Position.X + positionNormalMatrix.x * Value;
|
||||
// PositionCurrent.Position.Y = Axis.Loaction.Position.Y + positionNormalMatrix.y * Value;
|
||||
// PositionCurrent.Position.Z = Axis.Loaction.Position.Z + positionNormalMatrix.z * Value;
|
||||
// PositionCurrent.FixedAngle.R = Axis.Loaction.FixedAngle.R + positionNormalMatrix.rx * Value;
|
||||
// PositionCurrent.FixedAngle.P = Axis.Loaction.FixedAngle.P + positionNormalMatrix.ry * Value;
|
||||
// PositionCurrent.FixedAngle.Y = Axis.Loaction.FixedAngle.Y + positionNormalMatrix.rz * Value;
|
||||
|
||||
// return PositionCurrent;
|
||||
//}
|
||||
///// <summary>
|
||||
///// 根据界面数据，给位置变换标准矩阵赋值
|
||||
///// </summary>
|
||||
///// <returns></returns>
|
||||
//public PositionNormalMatrix SetPositionNormalMatrix()
|
||||
//{
|
||||
// positionNormalMatrix.x =0;
|
||||
// positionNormalMatrix.y =0;
|
||||
// positionNormalMatrix.z = 0;
|
||||
// positionNormalMatrix.rx = 0;
|
||||
// positionNormalMatrix.ry = 0;
|
||||
// positionNormalMatrix.rz = 0;
|
||||
|
||||
// switch (JointType)
|
||||
// {
|
||||
// case JointType.Revolute:
|
||||
// switch (Axis.AxisFlag)
|
||||
// {
|
||||
// case AxisFlag.X:
|
||||
// positionNormalMatrix.rx = 1;
|
||||
// break;
|
||||
// case AxisFlag.Y:
|
||||
// positionNormalMatrix.ry = 1;
|
||||
// break;
|
||||
// case AxisFlag.Z:
|
||||
// positionNormalMatrix.rz = 1;
|
||||
// break;
|
||||
// }
|
||||
// break;
|
||||
// case JointType.Prismatic:
|
||||
// switch (Axis.AxisFlag)
|
||||
// {
|
||||
// case AxisFlag.X:
|
||||
// positionNormalMatrix.x = 1;
|
||||
// break;
|
||||
// case AxisFlag.Y:
|
||||
// positionNormalMatrix.y = 1;
|
||||
// break;
|
||||
// case AxisFlag.Z:
|
||||
// positionNormalMatrix.z = 1;
|
||||
// break;
|
||||
// }
|
||||
// break;
|
||||
|
||||
// }
|
||||
// switch (Axis.Flip)
|
||||
// {
|
||||
// case 1:
|
||||
// positionNormalMatrix.x = -positionNormalMatrix.x;
|
||||
// positionNormalMatrix.y = -positionNormalMatrix.y;
|
||||
// positionNormalMatrix.z = -positionNormalMatrix.z;
|
||||
// positionNormalMatrix.rx = -positionNormalMatrix.rx;
|
||||
// positionNormalMatrix.ry = -positionNormalMatrix.rx;
|
||||
// positionNormalMatrix.rz = -positionNormalMatrix.rx;
|
||||
// break;
|
||||
// }
|
||||
// return positionNormalMatrix;
|
||||
//}
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endregion
|
||||
}
|
||||
|
||||
@@ -0,0 +1,78 @@
|
||||
using System;
|
||||
using System.Collections;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
|
||||
namespace KinematicsBase
|
||||
{
|
||||
#region BaseClass
|
||||
|
||||
|
||||
[Serializable]
|
||||
public class JointPose
|
||||
{
|
||||
/// <summary>
|
||||
/// 0
|
||||
/// </summary>
|
||||
public string JointId { get; set; }
|
||||
/// <summary>
|
||||
/// 1 名称
|
||||
/// </summary>
|
||||
public string JointName { get; set; }
|
||||
public string ModelId { get; set; }
|
||||
/// <summary>
|
||||
/// 2 值
|
||||
/// </summary>
|
||||
public double Value { get; set; }
|
||||
/// <summary>
|
||||
/// 3 速度
|
||||
/// </summary>
|
||||
public double Speed { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 4 驱动关节
|
||||
/// </summary>
|
||||
public bool IsDriving { get; set; }
|
||||
/// <summary>
|
||||
/// 5 倍率
|
||||
/// </summary>
|
||||
public double ValueFactor { get; set; }
|
||||
/// <summary>
|
||||
/// 6 加速
|
||||
/// </summary>
|
||||
public double AccelerationStart { get; set; }
|
||||
/// <summary>
|
||||
/// 7 减速
|
||||
/// </summary>
|
||||
public double AccelerationEnd { get; set; }
|
||||
/// <summary>
|
||||
/// 8 最小值
|
||||
/// </summary>
|
||||
public double LowLimit { get; set; }
|
||||
/// <summary>
|
||||
/// 9 最大值
|
||||
/// </summary>
|
||||
public double HighLimit { get; set; }
|
||||
/// <summary>
|
||||
/// 10 时间
|
||||
/// </summary>
|
||||
public int Interval { get; set; }
|
||||
public JointPose()
|
||||
{
|
||||
|
||||
}
|
||||
public JointPose(string JointId, string JointName)
|
||||
{
|
||||
this.JointId = JointId;
|
||||
this.JointName = JointName;
|
||||
Value = 0;
|
||||
Speed = 100;
|
||||
ValueFactor = 1;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endregion
|
||||
}
|
||||
@@ -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
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,58 @@
|
||||
using System;
|
||||
using System.Collections;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
namespace KinematicsBase
|
||||
{
|
||||
#region Link
|
||||
|
||||
|
||||
[Serializable]
|
||||
public class LinkElem
|
||||
{
|
||||
public LinkElem()
|
||||
{
|
||||
}
|
||||
|
||||
public LinkElem(string id, string name)
|
||||
{
|
||||
this.Id = id;
|
||||
this.Name = name;
|
||||
}
|
||||
|
||||
public string Id { get; set; }
|
||||
|
||||
public string Name { get; set; }
|
||||
}
|
||||
[Serializable]
|
||||
public class Link
|
||||
{
|
||||
public Link()
|
||||
{
|
||||
this.GraphPosition = new System.Drawing.Point(0, 0);
|
||||
}
|
||||
|
||||
public Link(string id, string name, bool isFixed = false)
|
||||
{
|
||||
this.Id = id;
|
||||
this.Name = name;
|
||||
this.IsFixed = isFixed;
|
||||
}
|
||||
|
||||
public System.Drawing.Point GraphPosition { set; get; }
|
||||
|
||||
public string Id { get; set; }
|
||||
|
||||
public string ModelId { get; set; }
|
||||
public bool IsFixed { get; set; }
|
||||
|
||||
public string Name { get; set; }
|
||||
|
||||
public List<LinkElem> LinkElements { get; set; } = new List<LinkElem>();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
#endregion
|
||||
}
|
||||
@@ -0,0 +1,178 @@
|
||||
using System;
|
||||
using System.Collections;
|
||||
using System.Collections.Generic;
|
||||
using System.IO;
|
||||
using System.Linq;
|
||||
using System.Windows.Forms;
|
||||
|
||||
namespace KinematicsBase
|
||||
{
|
||||
|
||||
public class OPERATION_FramesList
|
||||
{
|
||||
public C_OPERATION FramesListCreate(string startPoseId, string endPoseId, int Interval)
|
||||
{
|
||||
Pose startPose;
|
||||
Pose endPose;
|
||||
startPose = Kinematics.GetPoseByPoseId(startPoseId);
|
||||
endPose = Kinematics.GetPoseByPoseId(endPoseId);
|
||||
|
||||
if(startPose != null && endPose != null)
|
||||
{
|
||||
return FramesListCreate(startPose, endPose, Interval);
|
||||
}
|
||||
else
|
||||
{
|
||||
MessageBox.Show("姿态不存在", "提示");
|
||||
throw new Exception("Pose不存在");
|
||||
}
|
||||
|
||||
}
|
||||
public C_OPERATION FramesListCreate(Pose startPose, Pose endPose, int Interval)
|
||||
{
|
||||
// int framStep = 10;
|
||||
int framStep = PP.PCP.FrameValue;
|
||||
List<Pose> PoseList;
|
||||
PoseList = OPERATION_GetDifferencePoseList(startPose, endPose, Interval, framStep);
|
||||
return FramesCreate(PoseList);
|
||||
|
||||
}
|
||||
|
||||
public C_OPERATION FramesCreate(List<Pose> PoseList)
|
||||
{
|
||||
List<C_ObjStates> objStates;
|
||||
double timeIndex;
|
||||
int framStep = 10;
|
||||
C_Frames frames;
|
||||
C_OPERATION m_OPERATION_Frames = new C_OPERATION();
|
||||
for (int i = 0; i < PoseList.Count; i++)
|
||||
{
|
||||
Pose pose = PoseList[i];
|
||||
|
||||
timeIndex = (double)(framStep * i) / 1000;
|
||||
objStates = GetObjStates(pose);
|
||||
frames = new C_Frames();
|
||||
frames.time = timeIndex.ToString("0.00");
|
||||
frames.objStates = objStates;
|
||||
|
||||
m_OPERATION_Frames.frames.Add(frames);
|
||||
}
|
||||
return m_OPERATION_Frames;
|
||||
}
|
||||
#region Method
|
||||
|
||||
/// <summary>
|
||||
///
|
||||
/// </summary>
|
||||
/// <param name="startPose"></param>
|
||||
/// <param name="endPose"></param>
|
||||
/// <param name="Interval">单位毫秒</param>
|
||||
public List<Pose> OPERATION_GetDifferencePoseList(Pose startPose, Pose endPose, int Interval, int framStep)
|
||||
{
|
||||
List<Pose> PoseList = new List<Pose>();
|
||||
Pose pose;
|
||||
Dictionary<string, double> jointStep = new Dictionary<string, double>();
|
||||
int frameCount;
|
||||
|
||||
frameCount = Interval / framStep;
|
||||
//计算Joint的步长
|
||||
foreach (var item in endPose.PoseInfo)
|
||||
{
|
||||
double value = (endPose.PoseInfo[item.Key] - startPose.PoseInfo[item.Key]) / frameCount;
|
||||
jointStep.Add(item.Key, value);
|
||||
}
|
||||
for (int i = 0; i < frameCount; i++)
|
||||
{
|
||||
if (i == 0)
|
||||
{
|
||||
pose = new Pose();
|
||||
pose.ModelId = endPose.ModelId;
|
||||
pose.PoseInfo = new Dictionary<string, double>();
|
||||
foreach (var item in endPose.PoseInfo)
|
||||
{
|
||||
pose.PoseInfo.Add(item.Key, startPose.PoseInfo[item.Key]);
|
||||
}
|
||||
PoseList.Add(pose);
|
||||
}
|
||||
else if (i == frameCount - 1)
|
||||
{
|
||||
pose = new Pose();
|
||||
pose.ModelId = endPose.ModelId;
|
||||
pose.PoseInfo = new Dictionary<string, double>();
|
||||
foreach (var item in endPose.PoseInfo)
|
||||
{
|
||||
pose.PoseInfo.Add(item.Key, endPose.PoseInfo[item.Key]);
|
||||
}
|
||||
PoseList.Add(pose);
|
||||
}
|
||||
else
|
||||
{
|
||||
pose = new Pose();
|
||||
pose.ModelId = endPose.ModelId;
|
||||
pose.PoseInfo = new Dictionary<string, double>();
|
||||
foreach (var item in endPose.PoseInfo)
|
||||
{
|
||||
pose.PoseInfo.Add(item.Key, startPose.PoseInfo[item.Key] + i * jointStep[item.Key]);
|
||||
}
|
||||
PoseList.Add(pose);
|
||||
}
|
||||
}
|
||||
|
||||
return PoseList;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
public List<C_ObjStates> GetObjStates(Pose pose)
|
||||
{
|
||||
List<Joint> JointList = Kinematics.GetJointListByModelId(pose.ModelId);
|
||||
List<C_ObjStates> objStates = new List<C_ObjStates>();
|
||||
foreach (var item in pose.PoseInfo)
|
||||
{
|
||||
var joint = JointList.FirstOrDefault(t => t.Id == item.Key);
|
||||
|
||||
var objStateSubList = GetObjStates(pose.ModelId, joint.Id, item.Value);
|
||||
|
||||
foreach (var objState in objStateSubList)
|
||||
{
|
||||
objStates.Add(objState);
|
||||
}
|
||||
}
|
||||
return objStates;
|
||||
}
|
||||
|
||||
public List<C_ObjStates> GetObjStates(string modelId, string jId, double jValue)
|
||||
{
|
||||
List<Joint> JointList = Kinematics.GetJointListByModelId(modelId);
|
||||
List<C_ObjStates> objStates = new List<C_ObjStates>();
|
||||
//foreach (var item in pose.PoseInfo)
|
||||
var joint = JointList.FirstOrDefault(t => t.Id == jId);
|
||||
|
||||
//1.反向，0.正常
|
||||
if (joint.Axis.Flip == 1)
|
||||
{
|
||||
jValue = -jValue;
|
||||
}
|
||||
|
||||
//值按比例和初始值变换
|
||||
jValue = jValue * joint.ValueFactor + joint.Value;
|
||||
|
||||
//var value = ChangeToModelValue(JointList, jId, jValue);
|
||||
PositionQuaternion positionQuaternion = new Joint().TransformCoordinates(joint, jValue);
|
||||
C_ObjStates objState = new C_ObjStates();
|
||||
objState.i = joint.CId;
|
||||
objState.tx = positionQuaternion.Position.X.ToString("0.000000");
|
||||
objState.ty = positionQuaternion.Position.Y.ToString("0.000000");
|
||||
objState.tz = positionQuaternion.Position.Z.ToString("0.000000");
|
||||
objState.qx = positionQuaternion.Quaternion.X.ToString("0.000000");
|
||||
objState.qy = positionQuaternion.Quaternion.Y.ToString("0.000000");
|
||||
objState.qz = positionQuaternion.Quaternion.Z.ToString("0.000000");
|
||||
objState.qw = positionQuaternion.Quaternion.W.ToString("0.000000");
|
||||
objStates.Add(objState);
|
||||
return objStates;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,553 @@
|
||||
using System;
|
||||
using System.Collections;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
namespace KinematicsBase
|
||||
{
|
||||
#region BaseClass
|
||||
#region Data Define
|
||||
/// <summary>
|
||||
/// 传感器类型
|
||||
/// </summary>
|
||||
[Serializable]
|
||||
public enum SensorType
|
||||
{
|
||||
/// <summary>
|
||||
/// 姿态传感器
|
||||
/// </summary>
|
||||
Pose,
|
||||
/// <summary>
|
||||
/// 关节距离传感器
|
||||
/// </summary>
|
||||
Distance,
|
||||
/// <summary>
|
||||
/// 关节传感器
|
||||
/// 判断关节位置
|
||||
/// </summary>
|
||||
Joint,
|
||||
/// <summary>
|
||||
/// 接近传感器
|
||||
/// </summary>
|
||||
Near,
|
||||
/// <summary>
|
||||
/// 光电传感器
|
||||
/// </summary>
|
||||
PhotoElectric,
|
||||
/// <summary>
|
||||
/// 属性传感器
|
||||
/// </summary>
|
||||
Attribution,
|
||||
/// <summary>
|
||||
/// Joint的值变化 ture
|
||||
/// 不变化 false
|
||||
/// </summary>
|
||||
IsRun
|
||||
|
||||
}
|
||||
/// <summary>
|
||||
/// 信号触发方式
|
||||
/// </summary>
|
||||
/// [Serializable]
|
||||
public enum TrigerType
|
||||
{
|
||||
Pulse,
|
||||
Limit,
|
||||
StepBegin,
|
||||
StepEnd
|
||||
}
|
||||
[Serializable]
|
||||
|
||||
public enum SensorDataType
|
||||
{
|
||||
|
||||
INT,
|
||||
|
||||
DINT,
|
||||
|
||||
FLOAT,
|
||||
|
||||
DOUBLE
|
||||
}
|
||||
#endregion
|
||||
/// <summary>
|
||||
/// 姿态传感器
|
||||
/// </summary>
|
||||
[Serializable]
|
||||
public class Sensor
|
||||
{
|
||||
///// <summary>
|
||||
///// 向外部发出传感器信号
|
||||
///// </summary>
|
||||
//public static event Action<string, object> OnDataChange = null;
|
||||
|
||||
/// <summary>
|
||||
/// string ModeId, string PoseId, string JointId, string Id, object Value
|
||||
/// </summary>
|
||||
public static event Action<string, string, string, string, object> OnDataChange;
|
||||
//public static event Action<string, string, string, string, bool> OnDataChangeBool;
|
||||
//public static event Action<string, string, string, string, int> OnDataChangeInt;
|
||||
//public static event Action<string, string, string, string, double> OnDataChangeFloat;
|
||||
|
||||
public string Id { get; set; }
|
||||
|
||||
public string Name { get; set; }
|
||||
public string ModelId { get; set; }
|
||||
public bool I = false;
|
||||
public SensorType SensorType { get; set; }
|
||||
public TrigerType TrigerType { get; set; }
|
||||
public string PoseId { get; set; } = "";
|
||||
/// <summary>
|
||||
/// PoseId =0和姿态距离传感器用
|
||||
/// </summary>
|
||||
public string JointId { get; set; } = "";
|
||||
public UnitType UnitType { get; set; }
|
||||
public SensorDataType SensorDataType { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 传感器是否启用
|
||||
/// </summary>
|
||||
public bool IsEnable { get; set; }
|
||||
///// <summary>
|
||||
///// PLC 变量代码
|
||||
///// </summary>
|
||||
//public string SignalViewer_Id;
|
||||
|
||||
/// <summary>
|
||||
/// 姿态传感器 光电传感器
|
||||
/// 触发后返回值 0，1
|
||||
/// 或伺服值
|
||||
/// </summary>
|
||||
public object Tagvalue { get; set; }
|
||||
/// <summary>
|
||||
/// 触发值测试误差值上限
|
||||
/// </summary>
|
||||
public double LimitUpValue { get; set; } = 2f;
|
||||
/// <summary>
|
||||
/// 触发值测试误差值下限
|
||||
/// </summary>
|
||||
public double LimitDownValue { get; set; } = -2f;
|
||||
|
||||
/// <summary>
|
||||
/// 光电传感器 检测长度用
|
||||
/// </summary>
|
||||
public double Length { get; set; }
|
||||
public double ValueFactor { get; set; } = 1;
|
||||
|
||||
public Sensor()
|
||||
{
|
||||
this.Id = Guid.NewGuid().ToString("D").ToUpper();
|
||||
|
||||
}
|
||||
public void Setup()
|
||||
{
|
||||
switch (SensorType)
|
||||
{
|
||||
case SensorType.Pose:
|
||||
SetupPose();
|
||||
break;
|
||||
case SensorType.Distance:
|
||||
SetupDistance();
|
||||
break;
|
||||
case SensorType.Joint:
|
||||
SetupJoint();
|
||||
break;
|
||||
}
|
||||
}
|
||||
private void SetupJoint()
|
||||
{
|
||||
bool isTriger = false;
|
||||
//Kinematics k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == ModelId);
|
||||
var k = PP.ModelCodeFindModelAttribute(ModelId).KinematicsData;
|
||||
if (k == null) return;
|
||||
var val = k.JointList.FirstOrDefault(t => t.Id == JointId).ValueCurrent;
|
||||
var jValue = k.PoseList.FirstOrDefault(t => t.Id == PoseId).PoseInfo[JointId];
|
||||
|
||||
if ((val >= jValue + LimitDownValue) && (val <= jValue + LimitDownValue))
|
||||
{
|
||||
isTriger = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
isTriger = false;
|
||||
}
|
||||
I = isTriger;
|
||||
DataChange(ModelId, PoseId, JointId, Id, I);
|
||||
}
|
||||
|
||||
private void SetupDistance()
|
||||
{
|
||||
// Kinematics k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == ModelId);
|
||||
var k = PP.ModelCodeFindModelAttribute(ModelId).KinematicsData;
|
||||
if (k == null) return;
|
||||
double val = k.JointList.FirstOrDefault(t => t.Id == JointId).ValueCurrent;
|
||||
|
||||
object value;
|
||||
|
||||
switch (SensorDataType)
|
||||
{
|
||||
case SensorDataType.INT:
|
||||
value = Convert.ToInt32(val);
|
||||
DataChange(ModelId, PoseId, JointId, Id, value);
|
||||
|
||||
break;
|
||||
case SensorDataType.DINT:
|
||||
value = (long)val;
|
||||
DataChange(ModelId, PoseId, JointId, Id, value);
|
||||
break;
|
||||
case SensorDataType.FLOAT:
|
||||
value = (double)val;
|
||||
DataChange(ModelId, PoseId, JointId, Id, value);
|
||||
break;
|
||||
case SensorDataType.DOUBLE:
|
||||
value = (double)val;
|
||||
DataChange(ModelId, PoseId, JointId, Id, value);
|
||||
break;
|
||||
}
|
||||
}
|
||||
private void SetupPose()
|
||||
{
|
||||
//double val = (double)vale;
|
||||
bool isTriger = false;
|
||||
bool isTrigerJoint;
|
||||
int index = 0;
|
||||
//Kinematics k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == ModelId);
|
||||
var k = PP.ModelCodeFindModelAttribute(ModelId).KinematicsData;
|
||||
if (k == null) return;
|
||||
|
||||
//监控POSE的全部姿态
|
||||
var pose = k.PoseList.FirstOrDefault(t => t.Id == PoseId);
|
||||
if (pose == null) return;
|
||||
foreach (var item in pose.PoseInfo)
|
||||
{
|
||||
double currentValue = k.JointList.FirstOrDefault(t => t.Id == item.Key).ValueCurrent;
|
||||
if ((currentValue >= item.Value + LimitDownValue) && (currentValue <= item.Value + LimitUpValue))
|
||||
{
|
||||
isTrigerJoint = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
isTrigerJoint = false;
|
||||
}
|
||||
if (!isTrigerJoint)
|
||||
{
|
||||
isTriger = false;
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (index == 0)
|
||||
{
|
||||
isTriger = isTrigerJoint;
|
||||
}
|
||||
else
|
||||
{
|
||||
isTriger = isTriger && isTrigerJoint;
|
||||
}
|
||||
if (isTriger == false)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
index++;
|
||||
}
|
||||
I = isTriger;
|
||||
DataChange(ModelId, PoseId, JointId, Id, I);
|
||||
}
|
||||
public Sensor(SensorType SensorType)
|
||||
{
|
||||
this.Id = Guid.NewGuid().ToString("D").ToUpper();
|
||||
Tagvalue = 0;
|
||||
switch (SensorType)
|
||||
{
|
||||
case SensorType.IsRun:
|
||||
Joint.OnDataChangeJointIsRun -= Joint_OnDataChangeJointIsRun;
|
||||
Joint.OnDataChangeJointIsRun += Joint_OnDataChangeJointIsRun;
|
||||
break;
|
||||
default:
|
||||
Joint.OnDataChange -= Joint_OnDataChange;
|
||||
Joint.OnDataChange += Joint_OnDataChange;
|
||||
break;
|
||||
}
|
||||
}
|
||||
public void Joint_OnDataChangeJointIsRun(string modelId, string jointId, bool val)
|
||||
{
|
||||
if (this.ModelId == modelId && this.JointId == jointId)
|
||||
{
|
||||
I = val;
|
||||
DataChange(ModelId, PoseId, JointId, Id, I);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
public string CreateSensorPose(string modelId, string poseId)
|
||||
{
|
||||
this.Id = Guid.NewGuid().ToString("D").ToUpper();
|
||||
Joint.OnDataChange -= Joint_OnDataChange;
|
||||
Joint.OnDataChange += Joint_OnDataChange;
|
||||
Tagvalue = 0;
|
||||
|
||||
this.IsEnable = true;
|
||||
this.PoseId = poseId;
|
||||
var kTemp = Kinematics.GetKinematicsByModelId(modelId);
|
||||
{
|
||||
this.Name = kTemp.ModelName + "_PS_Logical_1";
|
||||
this.SensorType = SensorType.Pose;
|
||||
}
|
||||
this.ModelId = modelId;
|
||||
this.LimitUpValue = 2;
|
||||
this.LimitDownValue = -2;
|
||||
kTemp.SensorList.Add(this);
|
||||
// Kinematics.SaveToModel(kTemp);
|
||||
if (GlobalEventInterface.E_Transmit_SaveToModel != null)
|
||||
{
|
||||
GlobalEventInterface.E_Transmit_SaveToModel(kTemp);
|
||||
}
|
||||
|
||||
return Id;
|
||||
}
|
||||
public string CreateSensorJoint(string modelId, string poseId, string jointId)
|
||||
{
|
||||
this.Id = Guid.NewGuid().ToString("D").ToUpper();
|
||||
Joint.OnDataChange -= Joint_OnDataChange;
|
||||
Joint.OnDataChange += Joint_OnDataChange;
|
||||
Tagvalue = 0;
|
||||
this.IsEnable = true;
|
||||
this.PoseId = poseId;
|
||||
this.JointId = jointId;
|
||||
var kTemp = Kinematics.GetKinematicsByModelId(modelId);
|
||||
|
||||
this.Name = kTemp.ModelName + "_PJ_Logical_1";
|
||||
this.SensorType = SensorType.Joint;
|
||||
this.ModelId = modelId;
|
||||
this.LimitUpValue = 2;
|
||||
this.LimitDownValue = -2;
|
||||
kTemp.SensorList.Add(this);
|
||||
// Kinematics.SaveToModel(kTemp);
|
||||
if (GlobalEventInterface.E_Transmit_SaveToModel != null)
|
||||
{
|
||||
GlobalEventInterface.E_Transmit_SaveToModel(kTemp);
|
||||
}
|
||||
return Id;
|
||||
}
|
||||
public string CreateSensorJointIsRun(string modelId, string jointId)
|
||||
{
|
||||
this.Id = Guid.NewGuid().ToString("D").ToUpper();
|
||||
Joint.OnDataChangeJointIsRun -= Joint_OnDataChangeJointIsRun;
|
||||
Joint.OnDataChangeJointIsRun += Joint_OnDataChangeJointIsRun;
|
||||
|
||||
Tagvalue = 0;
|
||||
|
||||
this.IsEnable = true;
|
||||
this.JointId = jointId;
|
||||
var kTemp = Kinematics.GetKinematicsByModelId(modelId);
|
||||
|
||||
this.Name = kTemp.ModelName + "_Run_Logical_1";
|
||||
this.SensorType = SensorType.IsRun;
|
||||
this.ModelId = modelId;
|
||||
kTemp.SensorList.Add(this);
|
||||
// Kinematics.SaveToModel(kTemp);
|
||||
if (GlobalEventInterface.E_Transmit_SaveToModel != null)
|
||||
{
|
||||
GlobalEventInterface.E_Transmit_SaveToModel(kTemp);
|
||||
}
|
||||
return Id;
|
||||
}
|
||||
public string CreateSensorPoseDistance(string modelId, string jointId, SensorDataType sensorDataType)
|
||||
{
|
||||
this.Id = Guid.NewGuid().ToString("D").ToUpper();
|
||||
Joint.OnDataChange -= Joint_OnDataChange;
|
||||
Joint.OnDataChange += Joint_OnDataChange;
|
||||
Tagvalue = 0;
|
||||
|
||||
var kTemp = Kinematics.GetKinematicsByModelId(modelId);
|
||||
|
||||
this.IsEnable = true;
|
||||
this.JointId = jointId;
|
||||
this.SensorType = SensorType.Distance;
|
||||
this.Name = kTemp.ModelName + "_JD_Logical_1";
|
||||
this.ModelId = modelId;
|
||||
this.SensorDataType = sensorDataType;
|
||||
this.LimitUpValue = 2;
|
||||
this.LimitDownValue = -2;
|
||||
kTemp.SensorList.Add(this);
|
||||
// Kinematics.SaveToModel(kTemp);
|
||||
if (GlobalEventInterface.E_Transmit_SaveToModel != null)
|
||||
{
|
||||
GlobalEventInterface.E_Transmit_SaveToModel(kTemp);
|
||||
}
|
||||
return Id;
|
||||
}
|
||||
|
||||
public static void DataChange(string ModeId, string PoseId, string JointId, string Id, object Value)
|
||||
{
|
||||
if (OnDataChange != null)
|
||||
{
|
||||
OnDataChange.Invoke(ModeId, PoseId, JointId, Id, Value);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
public void Joint_OnDataChange(string modeId, string jointID, object val)
|
||||
{
|
||||
//if (this.Id == null) return;
|
||||
if (this.ModelId == modeId)
|
||||
{
|
||||
Triger(jointID, val);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 根据Joint姿态的值，触发传感器信号
|
||||
/// </summary>
|
||||
/// <param name="val"></param>
|
||||
public void Triger(string jointID, object val)
|
||||
{
|
||||
switch (SensorType)
|
||||
{
|
||||
case SensorType.Pose:
|
||||
TrigerPose(val);
|
||||
break;
|
||||
case SensorType.Distance:
|
||||
TrigerDistance(jointID, val);
|
||||
break;
|
||||
case SensorType.Joint:
|
||||
TrigerJoint(jointID, val);
|
||||
break;
|
||||
case SensorType.PhotoElectric:
|
||||
break;
|
||||
case SensorType.Near:
|
||||
break;
|
||||
case SensorType.Attribution:
|
||||
break;
|
||||
}
|
||||
}
|
||||
/// <summary>
|
||||
/// 回写关节当前值（距离）
|
||||
/// 可以订阅SensorDistanceDataChange事件
|
||||
/// </summary>
|
||||
/// <param name="val"></param>
|
||||
public void TrigerDistance(string jointID, object val)
|
||||
{
|
||||
if (this.JointId != jointID) return;
|
||||
object value;
|
||||
|
||||
switch (SensorDataType)
|
||||
{
|
||||
case SensorDataType.INT:
|
||||
value = Convert.ToInt32(val);
|
||||
DataChange(ModelId, PoseId, JointId, Id, value);
|
||||
|
||||
break;
|
||||
case SensorDataType.DINT:
|
||||
value = (long)val;
|
||||
DataChange(ModelId, PoseId, JointId, Id, value);
|
||||
break;
|
||||
case SensorDataType.FLOAT:
|
||||
value = (double)val;
|
||||
DataChange(ModelId, PoseId, JointId, Id, value);
|
||||
break;
|
||||
case SensorDataType.DOUBLE:
|
||||
value = (double)val;
|
||||
DataChange(ModelId, PoseId, JointId, Id, value);
|
||||
break;
|
||||
}
|
||||
}
|
||||
/// <summary>
|
||||
/// 判断姿态位置
|
||||
/// </summary>
|
||||
/// <param name="val"></param>
|
||||
public void TrigerPose(object vale)
|
||||
{
|
||||
double val = (double)vale;
|
||||
bool isTriger = false;
|
||||
bool isTrigerJoint;
|
||||
int index = 0;
|
||||
//Kinematics k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == ModelId);
|
||||
var k = PP.ModelCodeFindModelAttribute(ModelId).KinematicsData;
|
||||
if (k == null) return;
|
||||
|
||||
//监控POSE的全部姿态
|
||||
var pose = k.PoseList.FirstOrDefault(t => t.Id == PoseId);
|
||||
if (pose == null) return;
|
||||
foreach (var item in pose.PoseInfo)
|
||||
{
|
||||
double currentValue = k.JointList.FirstOrDefault(t => t.Id == item.Key).ValueCurrent;
|
||||
if ((currentValue >= item.Value + LimitDownValue) && (currentValue <= item.Value + LimitUpValue))
|
||||
{
|
||||
isTrigerJoint = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
isTrigerJoint = false;
|
||||
}
|
||||
if (!isTrigerJoint)
|
||||
{
|
||||
isTriger = false;
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (index == 0)
|
||||
{
|
||||
isTriger = isTrigerJoint;
|
||||
}
|
||||
else
|
||||
{
|
||||
isTriger = isTriger && isTrigerJoint;
|
||||
}
|
||||
if (isTriger == false)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
index++;
|
||||
}
|
||||
if (I != isTriger)
|
||||
{
|
||||
I = isTriger;
|
||||
DataChange(ModelId, PoseId, JointId, Id, I);
|
||||
//DataChangeBool(ModelId, PoseId, JointId, Id, I);
|
||||
}
|
||||
}
|
||||
/// <summary>
|
||||
/// 关节传感器
|
||||
/// 判断关节位置
|
||||
/// </summary>
|
||||
/// <param name="jointID"></param>
|
||||
/// <param name="val"></param>
|
||||
public void TrigerJoint(string jointID, object vale)
|
||||
{
|
||||
if (this.JointId != jointID) return;
|
||||
|
||||
double val = (double)vale;
|
||||
bool isTriger = false;
|
||||
// Kinematics k = DataBaseJson.dataBaseJson.KinematicsList.FirstOrDefault(t => t.ModelId == ModelId);
|
||||
var k = PP.ModelCodeFindModelAttribute(ModelId).KinematicsData;
|
||||
if (k == null) return;
|
||||
var jValue = k.PoseList.FirstOrDefault(t => t.Id == PoseId).PoseInfo[jointID];
|
||||
|
||||
if ((val >= jValue + LimitDownValue) && (val <= jValue + LimitDownValue))
|
||||
{
|
||||
isTriger = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
isTriger = false;
|
||||
}
|
||||
if (I != isTriger)
|
||||
{
|
||||
I = isTriger;
|
||||
DataChange(ModelId, PoseId, JointId, Id, I);
|
||||
//DataChangeBool(ModelId, PoseId, JointId, Id, I);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
//
|
||||
#endregion
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user