/// <summary>
/// 根据工具名称获取工具参数
/// </summary>
/// <param name="strServerUrl">机械臂接口服务器地址</param>
/// <param name="strToolName">工具名称</param>
/// <param name="robotTool">工具参数</param>
/// <returns>true成功 false失败</returns>
public bool GetTool(string strServerUrl, string strToolName, out MetaData.ToolInEndDesc robotTool)
{
bool bfound = false;
MetaData.Rpy rpy = new MetaData.Rpy();
robotTool = new MetaData.ToolInEndDesc();
string strURL = strServerUrl + "api/gettools";
string data = HttpGet(strURL);
if (data != string.Empty)
{
try {
JArray toolArray = Newtonsoft.Json.Linq.JArray.Parse(data) as JArray;
foreach (JArray item in toolArray)
{
JObject toolJobject = Newtonsoft.Json.JsonConvert.DeserializeObject(item[0].ToString()) as JObject;
if (toolJobject["tool_name"].ToString() == strToolName)
{
robotTool.cartPos.x = double.Parse(toolJobject["x"].ToString());
robotTool.cartPos.y = double.Parse(toolJobject["y"].ToString());
robotTool.cartPos.z = double.Parse(toolJobject["z"].ToString());
rpy.rx = double.Parse(toolJobject["rx"].ToString());
rpy.ry = double.Parse(toolJobject["ry"].ToString());
rpy.rz = double.Parse(toolJobject["rz"].ToString());
//欧拉角转四元数
rs_rpy_to_quaternion(rshd, ref rpy, ref robotTool.orientation);
Console.WriteLine("tool.name=" + strToolName);
Console.WriteLine("tool.pos.x=" + robotTool.cartPos.x.ToString());
Console.WriteLine("tool.pos.y=" + robotTool.cartPos.y.ToString());
Console.WriteLine("tool.pos.z=" + robotTool.cartPos.z.ToString());
Console.WriteLine("tool.ori.w=" + robotTool.orientation.w.ToString());
Console.WriteLine("tool.ori.x=" + robotTool.orientation.x.ToString());
Console.WriteLine("tool.ori.y=" + robotTool.orientation.y.ToString());
Console.WriteLine("tool.ori.z=" + robotTool.orientation.z.ToString());
bfound = true;
break;
}
}
} catch (Exception ex) {
Console.WriteLine(ex.ToString());
}
}
else
{
Console.WriteLine("get tool error!");
}
return(bfound);
}
/// <summary>
/// 获取用户坐标系参数
/// </summary>
/// <param name="strServerUrl">机械臂接口服务器地址</param>
/// <param name="strCoordName">用户坐标系名称</param>
/// <param name="coord">用户坐标系参数</param>
/// <returns>true成功 false失败</returns>
public bool GetCoord(string strServerUrl, string strCoordName, ref MetaData.CoordCalibrate coord)
{
bool bfound = false;
MetaData.Rpy rpy = new MetaData.Rpy();
string strURL = strServerUrl + "api/getcoords";
string data = HttpGet(strURL);
if (data != string.Empty)
{
if (data != string.Empty)
{
try {
JArray coordArray = Newtonsoft.Json.Linq.JArray.Parse(data) as JArray;
foreach (JArray item in coordArray)
{
JObject coordJobject = Newtonsoft.Json.JsonConvert.DeserializeObject(item[0].ToString()) as JObject;
Console.WriteLine(coordJobject["coord_name"].ToString());
if (coordJobject["coord_name"].ToString() == strCoordName)
{
//method
coord.methods = Util.GetCoordMothodByName(coordJobject["method"].ToString());
//coord type
coord.coordType = Util.WorldCoordinate;
//coord point1~3
string[] point1 = coordJobject["point1"].ToString().Split(',');
coord.jointPara[0].jointRadian[0] = double.Parse(point1[7]);
coord.jointPara[0].jointRadian[1] = double.Parse(point1[8]);
coord.jointPara[0].jointRadian[2] = double.Parse(point1[9]);
coord.jointPara[0].jointRadian[3] = double.Parse(point1[10]);
coord.jointPara[0].jointRadian[4] = double.Parse(point1[11]);
coord.jointPara[0].jointRadian[5] = double.Parse(point1[12]);
string[] point2 = coordJobject["point2"].ToString().Split(',');
coord.jointPara[1].jointRadian[0] = double.Parse(point2[7]);
coord.jointPara[1].jointRadian[1] = double.Parse(point2[8]);
coord.jointPara[1].jointRadian[2] = double.Parse(point2[9]);
coord.jointPara[1].jointRadian[3] = double.Parse(point2[10]);
coord.jointPara[1].jointRadian[4] = double.Parse(point2[11]);
coord.jointPara[1].jointRadian[5] = double.Parse(point2[12]);
string[] point3 = coordJobject["point3"].ToString().Split(',');
coord.jointPara[2].jointRadian[0] = double.Parse(point3[7]);
coord.jointPara[2].jointRadian[1] = double.Parse(point3[8]);
coord.jointPara[2].jointRadian[2] = double.Parse(point3[9]);
coord.jointPara[2].jointRadian[3] = double.Parse(point3[10]);
coord.jointPara[2].jointRadian[4] = double.Parse(point3[11]);
coord.jointPara[2].jointRadian[5] = double.Parse(point3[12]);
//coordinate tool describle
coord.toolDesc.cartPos.x = double.Parse(coordJobject["x"].ToString());
coord.toolDesc.cartPos.y = double.Parse(coordJobject["y"].ToString());
coord.toolDesc.cartPos.z = double.Parse(coordJobject["z"].ToString());
//欧拉角转四元数
rs_rpy_to_quaternion(rshd, ref rpy, ref coord.toolDesc.orientation);
Console.WriteLine("coord.method=" + coord.methods.ToString());
Console.WriteLine("coord.point1=" + coordJobject["point1"].ToString());
Console.WriteLine("coord.point2=" + coordJobject["point2"].ToString());
Console.WriteLine("coord.point3=" + coordJobject["point3"].ToString());
Console.WriteLine("coord.tool.name=" + coordJobject["tool_name"].ToString());
Console.WriteLine("coord.tool.pos.x=" + coord.toolDesc.cartPos.x.ToString());
Console.WriteLine("coord.tool.pos.y=" + coord.toolDesc.cartPos.y.ToString());
Console.WriteLine("coord.tool.pos.z=" + coord.toolDesc.cartPos.z.ToString());
Console.WriteLine("coord.tool.ori.w=" + coord.toolDesc.orientation.w.ToString());
Console.WriteLine("coord.tool.ori.x=" + coord.toolDesc.orientation.x.ToString());
Console.WriteLine("coord.tool.ori.y=" + coord.toolDesc.orientation.y.ToString());
Console.WriteLine("coord.tool.ori.z=" + coord.toolDesc.orientation.z.ToString());
bfound = true;
break;
}
}
} catch (Exception ex) {
Console.WriteLine(ex.ToString());
}
}
else
{
Console.WriteLine("get coordinate error!");
}
}
return(bfound);
}
public static extern int rs_quaternion_to_rpy(UInt16 rshd, ref MetaData.Ori ori, ref MetaData.Rpy rpy);
public static extern int rs_rpy_to_quaternion(UInt16 rshd, ref MetaData.Rpy rpy, ref MetaData.Ori ori);
