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
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user