524 lines
18 KiB
C#
524 lines
18 KiB
C#
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
|
||
}
|
||
|