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); } } }
//打印路点信息 public static void PrintWaypoint(MetaData.wayPoint_S point) { Console.Out.WriteLine("---------------------------------------------------------------------------------------"); Console.Out.WriteLine("pos.x={0} y={1} z={2}", point.cartPos.x, point.cartPos.y, point.cartPos.z); Console.Out.WriteLine("ori.w={0} x={1} y={2} z={3}", point.orientation.w, point.orientation.x, point.orientation.y, point.orientation.z); Console.Out.WriteLine("joint1={0} joint2={1} joint3={2}", point.jointpos[0] * 180 / Util.M_PI, point.jointpos[1] * 180 / Util.M_PI, point.jointpos[2] * 180 / Util.M_PI); Console.Out.WriteLine("joint4={0} joint5={1} joint6={2}", point.jointpos[3] * 180 / Util.M_PI, point.jointpos[4] * 180 / Util.M_PI, point.jointpos[5] * 180 / Util.M_PI); Console.Out.WriteLine("---------------------------------------------------------------------------------------"); }
//回调函数 static void CurrentPositionCallback(ref MetaData.wayPoint_S waypoint, IntPtr arg) { PrintWaypoint(waypoint); }
public static extern int rs_inverse_kin(UInt16 rshd, double[] joint_radia, ref MetaData.Pos pos, ref MetaData.Ori ori, ref MetaData.wayPoint_S waypoint);
public static extern int rs_forward_kin(UInt16 rshd, double[] joint_radia, ref MetaData.wayPoint_S waypoint);
public static extern int rs_get_current_waypoint(UInt16 rshd, ref MetaData.wayPoint_S waypoint);