Exemplo n.º 1
0
        private void MainForm_Load(object sender, EventArgs e)
        {
            // Ok, let's start to build our virtual robot arm !

            model = new DenavitHartenbergModel(new Vector3(0, 0, 0));

            // Add the first joint 
            model.Joints.Add(alpha: 0, theta: Math.PI / 4, radius: 35, offset: 0);

            // Add the second joint
            model.Joints.Add(alpha: 0, theta: -Math.PI / 3, radius: 35, offset: 0);

            // Create the top finger
            model_tgripper = new DenavitHartenbergModel();
            model_tgripper.Joints.Add(alpha: 0, theta: Math.PI / 4, radius: 20, offset: 0);
            model_tgripper.Joints.Add(alpha: 0, theta: -Math.PI / 3, radius: 20, offset: 0);

            // Create the bottom finger
            model_bgripper = new DenavitHartenbergModel();
            model_bgripper.Joints.Add(alpha: 0, theta: -Math.PI / 4, radius: 20, offset: 0);
            model_bgripper.Joints.Add(alpha: 0, theta: Math.PI / 3, radius: 20, offset: 0);

            // Create the model combinator from the parent model
            arm = new DenavitHartenbergNode(model);

            // Add the top finger
            arm.Children.Add(model_tgripper);

            // Add the bottom finger
            arm.Children.Add(model_bgripper);

            // Calculate the whole model (parent model + children models)
            arm.Compute();

            // Create the model visualizer
            viewer = new DenavitHartenbergViewer(pictureBox1.Width, pictureBox1.Height);

            // Assign each projection image of the model to a picture box
            pictureBox1.Image = viewer.PlaneXY;
            pictureBox2.Image = viewer.PlaneXZ;
            pictureBox3.Image = viewer.PlaneYZ;

            // Start the animation
            timer1.Interval = 40;
            timer1.Enabled = true;
        }
Exemplo n.º 2
0
        /// <summary>
        ///   Computes the three images of a list of models
        /// </summary>
        ///
        /// <param name="model">List of arguments of models</param>
        ///
        public void ComputeImages(params DenavitHartenbergModel[] model)
        {
            // Clear the 3 images with the background color
            gxy.Clear(BackColor);
            gxz.Clear(BackColor);
            gyz.Clear(BackColor);

            // Draw each model
            for (int i = 0; i < model.Length; i++)
            {
                DenavitHartenbergModel mdl = model[i];

                Pen pLinks = new Pen(LinksColor, 1);

                // Draw each link of the model
                Vector3 previous = mdl.Position;

                for (int j = 0; j < mdl.Joints.Count; j++)
                {
                    Vector3 current = mdl.Joints[j].Position;

                    // XY
                    gxy.DrawLine(pLinks,
                                 (xy.Width / 2) + previous.X * Scale,
                                 (xy.Height / 2) - previous.Y * Scale,
                                 (xy.Width / 2) + current.X * Scale,
                                 (xy.Height / 2) - current.Y * Scale);

                    // ZY
                    gyz.DrawLine(pLinks,
                                 (yz.Width / 2) + previous.Z * Scale,
                                 (yz.Height / 2) - previous.Y * Scale,
                                 (yz.Width / 2) + current.Z * Scale,
                                 (yz.Height / 2) - current.Y * Scale);

                    // XZ
                    gxz.DrawLine(pLinks,
                                 (xz.Width / 2) + previous.X * Scale,
                                 (xz.Height / 2) - previous.Z * Scale,
                                 (xz.Width / 2) + current.X * Scale,
                                 (xz.Height / 2) - current.Z * Scale);

                    previous = current;
                }

                Brush pJoints = new SolidBrush(JointsColor);

                // Draw each joint
                for (int j = 0; j < mdl.Joints.Count + 1; j++)
                {
                    Vector3 current;

                    // Select the color of the joint
                    if (j == 0)
                    {
                        pJoints = new SolidBrush(BaseJointColor);
                        current = mdl.Position;
                    }
                    else
                    {
                        current = mdl.Joints[j - 1].Position;

                        if (j == mdl.Joints.Count)
                        {
                            pJoints = new SolidBrush(EndJointColor);
                        }
                        else
                        {
                            pJoints = new SolidBrush(JointsColor);
                        }
                    }

                    // XY
                    gxy.FillEllipse(pJoints,
                                    (xy.Width / 2) + current.X * Scale - (JointRadius / 2),
                                    (xy.Height / 2) - current.Y * Scale - (JointRadius / 2),
                                    JointRadius, JointRadius);

                    // YZ
                    gyz.FillEllipse(pJoints,
                                    (yz.Width / 2) + current.Z * Scale - (JointRadius / 2),
                                    (yz.Height / 2) - current.Y * Scale - (JointRadius / 2),
                                    JointRadius, JointRadius);

                    // XZ
                    gxz.FillEllipse(pJoints,
                                    (xz.Width / 2) + current.X * Scale - (JointRadius / 2),
                                    (xz.Height / 2) - current.Z * Scale - (JointRadius / 2),
                                    JointRadius, JointRadius);
                }
            }

            // Draw the arrows on the windows
            DrawArrows(ref gxy, "Y", "X");
            DrawArrows(ref gyz, "Y", "Z");
            DrawArrows(ref gxz, "Z", "X");
        }