编辑FunnyMap对象

请找管理员授权。/funnyscript/edit_node_item.php

using Common_Robot2;
using ConverxHull;
using System.Numerics;
using System.Text;

namespace Test1
{

/// <summary>
    /// 搬家到 Cal_Camera_In_Arm_Matrix_S35
    /// </summary>
    public class Cal_Camera_In_Arm_Matrix_S35 : C_Node
    {
        public string? file= "D:\\data\\dazu_20241009.txt";
        public string? file_output1 = "D:\\Data\\calibration\\virtual_2_camera.txt";
        public string? file_output2 = "D:\\Data\\calibration\\camera_2_virtual.txt";
        public string? key_robot = "";

public Cal_Camera_In_Arm_Matrix_S35(string name, C_Space space_parent, C_Space space) : 
            base(name,space_parent, space)
        {
        }

public override void init()
        {
        }

public override Task run_sub()
        {
            run_sub_main();
            return Task.CompletedTask;
        }

/// <summary>
        /// 虚拟相机1，和机械臂只是一个平移关系
        /// 虚拟相机2，和虚拟相机1是一个旋转关系
        /// 虚拟相机2 到相机的变换为M
        /// 
        /// file format 摄像头坐标xyz,6轴,机械臂坐标xyz
        /// </summary>
        public void run_sub_main()
        {

C_Robot_DaZu_S35 pRobot2 = (C_Robot_DaZu_S35)this.read_var(this.key_robot, "");
            if (pRobot2 == null)
            {
                Main.speak_async("机器人参数为空！");
                return;
            }

string strFile = this.read_string(this.file);
            string? line;
            StreamReader? streamReader = new StreamReader(strFile);
            List<string> list=new List<string>();
            while ((line = streamReader?.ReadLine()) != null)
            {
                if (line.StartsWith("//"))
                {
                    continue;
                }

line = line.Replace("，", ",");
                line = line.Replace(" ", ",");
                line = line.Replace(",,", ",");
                string[] array = line.Split(',');
                if (array.Length == 12)
                {
                    list.Add(line);
                }
            }
            streamReader?.Close();

//机械臂坐标
            float arm_x = 0;
            float arm_y = 0;
            float arm_z = 0;
            //arm_x = -215.936f;//f,
            //arm_y = -949.207f;//,-249.857 - 1286;
            //arm_z = -249.414f;// - 246;

StringBuilder sb = new StringBuilder();
            StringBuilder sb2 = new StringBuilder();
            for (var k=0;k<list.Count; k++)
            {
                line = list[k]; 
                string[] strSplit = line.Split(",");
                float[] angles = new float[6];

for (var i = 0; i < Math.Min(angles.Length, strSplit.Length); i++)
                {
                    angles[i] = float.Parse(strSplit[i+3].Trim());
                }

arm_x = float.Parse(strSplit[9]);
                arm_y = float.Parse(strSplit[10]);
                arm_z = float.Parse(strSplit[11]);

(Three.Net.Math.Matrix4 m1, Three.Net.Math.Matrix4 m2, C_Point3D vX, C_Point3D vY, C_Point3D vZ) 
                    = Main_Robot.DaZu_S35_机械臂变换矩阵(pRobot2,angles);
                C_Point3D p_x0 =  new C_Point3D(arm_x, arm_y, arm_z);
                C_Point3D p_x =  m1.Multiply_Point(p_x0);
                C_Point3D p_y = m2.Multiply_Point(p_x);

float db_x = float.Parse(strSplit[0]);
                float db_y = float.Parse(strSplit[1]);
                float db_z = float.Parse(strSplit[2]);
                Vector3 camera_3d = new Vector3(db_x, db_y, db_z);

string str_line = p_y.ToString() + "," + camera_3d.X+","+ camera_3d.Y + ","+ camera_3d.Z;
                string str_line2 = camera_3d.X + "," + camera_3d.Y + "," + camera_3d.Z + "," + p_y.ToString();
                sb.Append(str_line + "\r\n");
                sb2.Append(str_line2+"\r\n");

Console.WriteLine(p_x);
                Console.WriteLine(vX);

for (var i = 0; i < pRobot2.rotate_center.Length; i++)
                {
                    Vector3 tmp = pRobot2.rotate_center[i];
                    C_Point3D tmpP = new C_Point3D(tmp.X, tmp.Y, tmp.Z);
                    Console.WriteLine(i + "=" + tmpP.ToString());
                }
            }

string file1 = this.read_string(file_output1);
            File.WriteAllText(file1, sb.ToString());
            string file2 = this.read_string(file_output2);
            File.WriteAllText(file2, sb2.ToString());
        }

}
}

ID=10726 Cal_Camera_In_Arm_Matrix_S35.cs
using Common_Robot2; using ConverxHull; using System.Numerics; using System.Text; namespace Test1 { /// <summary> /// 搬家到 Cal_Camera_In_Arm_Matrix_S35 /// </summary> public class Cal_Camera_In_Arm_Matrix_S35 : C_Node { public string? file= "D:\\data\\dazu_20241009.txt"; public string? file_output1 = "D:\\Data\\calibration\\virtual_2_camera.txt"; public string? file_output2 = "D:\\Data\\calibration\\camera_2_virtual.txt"; public string? key_robot = ""; public Cal_Camera_In_Arm_Matrix_S35(string name, C_Space space_parent, C_Space space) : base(name,space_parent, space) { } public override void init() { } public override Task run_sub() { run_sub_main(); return Task.CompletedTask; } /// <summary> /// 虚拟相机1，和机械臂只是一个平移关系 /// 虚拟相机2，和虚拟相机1是一个旋转关系 /// 虚拟相机2 到相机的变换为M /// /// file format 摄像头坐标xyz,6轴,机械臂坐标xyz /// </summary> public void run_sub_main() { C_Robot_DaZu_S35 pRobot2 = (C_Robot_DaZu_S35)this.read_var(this.key_robot, ""); if (pRobot2 == null) { Main.speak_async("机器人参数为空！"); return; } string strFile = this.read_string(this.file); string? line; StreamReader? streamReader = new StreamReader(strFile); List<string> list=new List<string>(); while ((line = streamReader?.ReadLine()) != null) { if (line.StartsWith("//")) { continue; } line = line.Replace("，", ","); line = line.Replace(" ", ","); line = line.Replace(",,", ","); string[] array = line.Split(','); if (array.Length == 12) { list.Add(line); } } streamReader?.Close(); //机械臂坐标 float arm_x = 0; float arm_y = 0; float arm_z = 0; //arm_x = -215.936f;//f, //arm_y = -949.207f;//,-249.857 - 1286; //arm_z = -249.414f;// - 246; StringBuilder sb = new StringBuilder(); StringBuilder sb2 = new StringBuilder(); for (var k=0;k<list.Count; k++) { line = list[k]; string[] strSplit = line.Split(","); float[] angles = new float[6]; for (var i = 0; i < Math.Min(angles.Length, strSplit.Length); i++) { angles[i] = float.Parse(strSplit[i+3].Trim()); } arm_x = float.Parse(strSplit[9]); arm_y = float.Parse(strSplit[10]); arm_z = float.Parse(strSplit[11]); (Three.Net.Math.Matrix4 m1, Three.Net.Math.Matrix4 m2, C_Point3D vX, C_Point3D vY, C_Point3D vZ) = Main_Robot.DaZu_S35_机械臂变换矩阵(pRobot2,angles); C_Point3D p_x0 = new C_Point3D(arm_x, arm_y, arm_z); C_Point3D p_x = m1.Multiply_Point(p_x0); C_Point3D p_y = m2.Multiply_Point(p_x); float db_x = float.Parse(strSplit[0]); float db_y = float.Parse(strSplit[1]); float db_z = float.Parse(strSplit[2]); Vector3 camera_3d = new Vector3(db_x, db_y, db_z); string str_line = p_y.ToString() + "," + camera_3d.X+","+ camera_3d.Y + ","+ camera_3d.Z; string str_line2 = camera_3d.X + "," + camera_3d.Y + "," + camera_3d.Z + "," + p_y.ToString(); sb.Append(str_line + "\r\n"); sb2.Append(str_line2+"\r\n"); Console.WriteLine(p_x); Console.WriteLine(vX); for (var i = 0; i < pRobot2.rotate_center.Length; i++) { Vector3 tmp = pRobot2.rotate_center[i]; C_Point3D tmpP = new C_Point3D(tmp.X, tmp.Y, tmp.Z); Console.WriteLine(i + "=" + tmpP.ToString()); } } string file1 = this.read_string(file_output1); File.WriteAllText(file1, sb.ToString()); string file2 = this.read_string(file_output2); File.WriteAllText(file2, sb2.ToString()); } } }