105 lines
3.1 KiB
C#
105 lines
3.1 KiB
C#
using DTTest;
|
||
using Newtonsoft.Json;
|
||
using Newtonsoft.Json.Linq;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using System.Text;
|
||
using System.Threading.Tasks;
|
||
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
namespace ModelBasic
|
||
{
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
public class ParsingData
|
||
{
|
||
|
||
/// <summary>
|
||
/// 在Json中得到Base、Tool、Tcp位置
|
||
/// 2021.1.15
|
||
/// </summary>
|
||
/// <param name="jsonStr">Json</param>
|
||
/// <param name="threejsBase">xyz rpy</param>
|
||
/// <param name="threejsTool">xyz qxqyqzqw</param>
|
||
/// <param name="threejsTcp">xyz qxqyqzqw</param>
|
||
/// <param name="threejsInitJoint">返回初始机器人姿态坐标值，传出的threejsInitJoint 是rad 和m</param>
|
||
public static void GetRobotParameter_MM(Data_RobotCurr robotCurr, out double[] threejsBaseQ, out double[] threejsToolQ, out double[] threejsTcpQ,out double[] threejsInitJoint)
|
||
{
|
||
threejsBaseQ = new double[] {
|
||
CVT.MToMM(robotCurr.Base.tx),
|
||
CVT.MToMM(robotCurr.Base.ty),
|
||
CVT.MToMM(robotCurr.Base.tz),
|
||
robotCurr.Base.qx,
|
||
robotCurr.Base.qy,
|
||
robotCurr.Base.qz,
|
||
robotCurr.Base.qw};
|
||
threejsToolQ = new double[] {
|
||
CVT.MToMM(robotCurr.Tool.tx),
|
||
CVT.MToMM(robotCurr.Tool.ty),
|
||
CVT.MToMM(robotCurr.Tool.tz),
|
||
robotCurr.Tool.qx,
|
||
robotCurr.Tool.qy,
|
||
robotCurr.Tool.qz,
|
||
robotCurr.Tool.qw};
|
||
threejsTcpQ = new double[] {
|
||
CVT.MToMM(robotCurr.Tcp.tx),
|
||
CVT.MToMM(robotCurr.Tcp.ty),
|
||
CVT.MToMM(robotCurr.Tcp.tz),
|
||
robotCurr.Tcp.qx,
|
||
robotCurr.Tcp.qy,
|
||
robotCurr.Tcp.qz,
|
||
robotCurr.Tcp.qw};
|
||
|
||
#region 处理初始joint
|
||
|
||
var currentJoint = robotCurr.CurrentJoint;
|
||
var jointNum = currentJoint.Count;
|
||
|
||
threejsInitJoint = new double[jointNum];
|
||
|
||
for (int i = 0; i < jointNum; i++)
|
||
{
|
||
var item = currentJoint[i];
|
||
double value = Convert.ToDouble(item.JValue);
|
||
// 转动副 从弧度转换为角度
|
||
if (item.JType == "0")
|
||
{
|
||
value = value * 180 / Math.PI;
|
||
}
|
||
else // 滑动副 从m转换为mm
|
||
{
|
||
//value = value * 1000;
|
||
}
|
||
threejsInitJoint[i] = value;
|
||
}
|
||
|
||
|
||
|
||
#endregion
|
||
}
|
||
|
||
|
||
|
||
|
||
/// <summary>
|
||
///
|
||
/// </summary>
|
||
/// <param name="dataSplit"></param>
|
||
/// <param name="items"></param>
|
||
/// <returns></returns>
|
||
public static JObject GetPositionDataInfo(string jStr)
|
||
{
|
||
var itemsArray = (JArray)JsonConvert.DeserializeObject(jStr);
|
||
var jObjectParam = itemsArray[0].ToString();
|
||
return (JObject)JsonConvert.DeserializeObject(jObjectParam);
|
||
}
|
||
|
||
|
||
|
||
}
|
||
}
|