static void Main(string[] args)
{
//机械臂控制上下文句柄
UInt16 rshd = 0xffff;
//创建机械臂控制对象
RobotAdepter robot = new RobotAdepter();
MetaData.wayPoint_S waypoint = new MetaData.wayPoint_S();
double[] target = { 0, 0, 0, 0, 0, 0 }; //注意这个里面的值是弧度！
//获取所有工具名称列表
List <string> toolNamelist;
MetaData.ToolInEndDesc robotTool;
robot.GetToolNameList(ServerInfo.SERVER_URL, out toolNamelist);
foreach (string name in toolNamelist)
{
robot.GetTool(ServerInfo.SERVER_URL, name, out robotTool);
}
//获取所有坐标系名称列表
List <string> coordNameList;
robot.GetCoordNameList(ServerInfo.SERVER_URL, out coordNameList);
foreach (string name in coordNameList)
{
MetaData.CoordCalibrate coord = new MetaData.CoordCalibrate();
//分配内存
IntPtr pt_coord = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(MetaData.CoordCalibrate)));
coord = (MetaData.CoordCalibrate)Marshal.PtrToStructure(pt_coord, typeof(MetaData.CoordCalibrate));
//获取坐标系参数
robot.GetCoord(ServerInfo.SERVER_URL, name, ref coord);
//必须释放内存否则会造成内存泄露!!!
Marshal.FreeHGlobal(pt_coord);
}
//机械臂运动控制示例
if (RobotAdepter.rs_create_context(ref rshd) == Util.RSERR_SUCC)
{
Console.Out.WriteLine("rshd={0}", rshd);
//链接机械臂服务器
if (RobotAdepter.rs_login(rshd, ServerInfo.robotIP, ServerInfo.serverPort) == Util.RSERR_SUCC)
{
Console.Out.WriteLine("login succ.");
//轴动到目标位置
RobotAdepter.rs_move_joint(rshd, target, true);
//直接获取机械臂当前位置信息
RobotAdepter.rs_get_current_waypoint(rshd, ref waypoint);
//打印路点信息
RobotAdepter.PrintWaypoint(waypoint);
RobotAdepter.rs_logout(rshd);
}
}
}
/// <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_user_to_base(UInt16 rshd, ref MetaData.Pos pos_onuser, ref MetaData.Ori ori_onuser, ref MetaData.CoordCalibrate user_coord, ref MetaData.ToolInEndDesc tool_pos, ref MetaData.Pos pos_onbase, ref MetaData.Ori ori_onbase);
public static extern int rs_set_teach_coord(UInt16 rshd, ref MetaData.CoordCalibrate user_coord);
public static extern int rs_set_relative_offset_on_user(UInt16 rshd, ref MetaData.MoveRelative relative, ref MetaData.CoordCalibrate user_coord);
public static extern int rs_check_user_coord(UInt16 rshd, ref MetaData.CoordCalibrate user_coord);
public static extern int rs_move_rotate(UInt16 rshd, ref MetaData.CoordCalibrate user_coord, ref MetaData.MoveRotateAxis rotate_axis, double rotate_angle, bool isblock);
