private void button1_Click(object sender, EventArgs e) { RobotArm arm = RobotArm.lynxmotionL6(); arm.links[1] = ArmLink.rotateY(arm.links[1], Math.PI / 4); arm.links[1] = ArmLink.rotateZ(arm.links[1], -Math.PI / 4); arm.links[3] = ArmLink.rotateY(arm.links[3], -Math.PI / 4); arm.links[2] = ArmLink.rotateY(arm.links[2], Math.PI / 4); double[][] effector = arm.linkPosition(4); double x = effector[0][0]; double y = effector[1][0]; double z = effector[2][0]; String str = Convert.ToString(x) + " , " + Convert.ToString(y) + " , " + Convert.ToString(z); effectorPositionText.Text = str; Graphics g = panel1.CreateGraphics(); int panelx = panel1.Width; int panely = panel1.Height; arm.drawArm(panelx, panely, g); }
public static ArmLink rotateZ(ArmLink link, double angle) { int i, rows = 0, cols = 0; double[][] Rz = new double[4][]; for (i = 0; i < 4; i++) { Rz[i] = new double[4]; } // assigning values to the rotation matrix Rz[0][0] = Math.Cos(angle); Rz[0][1] = -Math.Sin(angle); Rz[0][2] = Rz[0][3] = 0; Rz[1][0] = Math.Sin(angle); Rz[1][1] = Math.Cos(angle); Rz[1][2] = Rz[1][3] = 0; Rz[2][0] = Rz[2][1] = 0; Rz[2][2] = 1; Rz[2][3] = 0; Rz[3][0] = Rz[3][1] = Rz[3][2] = 0; Rz[3][3] = 1; // Rotation matrix about Z filled double totalangle = (link.anglez + angle >= 2 * Math.PI) ? link.anglez + angle - 2 * Math.PI : link.anglez + angle; ArmLink newlink = new ArmLink(link.anglex, link.angley, totalangle); newlink.T = mulMatrices(link.T, Rz, 4, 4, 4, ref rows, ref cols); return(newlink); }
public static ArmLink translateXYZ(ArmLink link, double Dx, double Dy, double Dz) { int i, rows = 0, cols = 0; double[][] Tmatrix; Tmatrix = new double[4][]; for (i = 0; i < 4; i++) { Tmatrix[i] = new double[4]; } // assigning values to the translation matrix Tmatrix[0][0] = 1; Tmatrix[0][1] = Tmatrix[0][2] = 0; Tmatrix[0][2] = 0; Tmatrix[0][3] = Dx; Tmatrix[1][0] = 0; Tmatrix[1][1] = 1; Tmatrix[1][2] = 0; Tmatrix[1][3] = Dy; Tmatrix[2][0] = 0; Tmatrix[2][1] = 0; Tmatrix[2][2] = 1; Tmatrix[2][3] = Dz; Tmatrix[3][0] = Tmatrix[3][1] = Tmatrix[3][2] = 0; Tmatrix[3][3] = 1; // Translationb matrix filled ArmLink newlink = new ArmLink(); newlink.T = Tmatrix; return(newlink); }
/// <summary> /// Get the Euler angles of a link /// </summary> /// <param name="linkindex"></param> /// <returns>Returns Euler angles as: Pitch, Yaw, Roll</returns> public EulerParams linkAngles(int linkindex) { int i, j, rowsc = 0, colsc = 0; double[] angles = new double[3]; double[][] R = new double[4][]; for (i = 0; i < 4; i++) { R[i] = new double[4]; for (j = 0; j < 4; j++) { R[i][j] = links[linkindex].T[i][j]; } } for (i = linkindex - 1; i > 0; i--) { R = ArmLink.mulMatrices(links[i].T, R, 4, 4, 4, ref rowsc, ref colsc); } EulerParams p = new EulerParams(); p.Yaw = Math.Atan2(-R[2][0], Math.Sqrt(Math.Pow(R[0][0], 2) + Math.Pow(R[1][0], 2))); // yaw (about y) p.Pitch = Math.Atan2(R[1][0] / Math.Cos(angles[1]), R[0][0] / Math.Cos(angles[1])); // pitch (about x) p.Roll = Math.Atan2(R[2][1] / Math.Cos(angles[1]), R[2][2] / Math.Cos(angles[1])); // roll (about z) return(p); }
public RobotArm(int linknum) { this.linknum = linknum; this.effectorwidth = 100; // 100% open links = new ArmLink[this.linknum]; links[0] = new ArmLink(); // universal link }
private void button4_Click(object sender, EventArgs e) { if (arm == null) { return; } int angdeg = (int)Link1RateNumeric.Value; double angle = angdeg * Math.PI / 180; arm.links[1] = ArmLink.rotateY(arm.links[1], angle); g.Clear(Color.White); arm.drawArmcyl(xdim, ydim, g); double[][] effector = arm.linkPosition(6); Link1AngleText.Text = Convert.ToString(arm.links[1].angley * 180 / Math.PI); double x = effector[0][0]; double y = effector[1][0]; double z = effector[2][0]; String str = Convert.ToString(x) + " , " + Convert.ToString(y) + " , " + Convert.ToString(z); effectorPositionText.Text = str; //double[] orientation = arm.linkAngles(6); //yawText.Text = Convert.ToString(orientation[0]*180/Math.PI); //pitchText.Text = Convert.ToString(orientation[1]*180/Math.PI); //rollText.Text = Convert.ToString(orientation[2]*180/Math.PI); EulerParams effectorOrientation = arm.linkAngles(6); yawText.Text = Convert.ToString(effectorOrientation.Yaw * 180 / Math.PI); pitchText.Text = Convert.ToString(effectorOrientation.Pitch * 180 / Math.PI); rollText.Text = Convert.ToString(effectorOrientation.Roll * 180 / Math.PI); if (connected) { if (alligned) { String cmd; if (Servo0BaseIndexRadio.Checked) { cmd = " #0 P"; } else { cmd = " #1 P"; } int basepos = RobotArm.baseServo(arm.links[1].angley); cmd += Convert.ToString(basepos); cmd += " T1000\r"; serialPort1.Write(cmd); } } }
public static RobotArm lynxmotionL6() { RobotArm arm = new RobotArm(7); arm.links[1] = ArmLink.translateXYZ(arm.links[0], 0, 6.7, 0); arm.links[2] = ArmLink.translateXYZ(arm.links[1], 0, 0, 0); arm.links[3] = ArmLink.translateXYZ(arm.links[2], 15, 0, 0); arm.links[4] = ArmLink.translateXYZ(arm.links[3], 17.5, 0, 0); arm.links[5] = ArmLink.translateXYZ(arm.links[4], 3, 0, 0); arm.links[6] = ArmLink.translateXYZ(arm.links[5], 4, 0, 0); return(arm); }
public void drawArmcyl(int xdim, int ydim, Graphics gr) { int rowsc = 0, colsc = 0; double H = 0; double R = 1; // drawing the effector now Effector effector = new Effector(3, 1.5, 2, 10, -3, 0); effector.opening = effectorwidth; effector.T = ArmLink.mulMatrices(links[5].T, links[6].T, 4, 4, 4, ref rowsc, ref colsc); effector.T = ArmLink.mulMatrices(links[4].T, effector.T, 4, 4, 4, ref rowsc, ref colsc); effector.T = ArmLink.mulMatrices(links[3].T, effector.T, 4, 4, 4, ref rowsc, ref colsc); effector.T = ArmLink.mulMatrices(links[2].T, effector.T, 4, 4, 4, ref rowsc, ref colsc); effector.T = ArmLink.mulMatrices(links[1].T, effector.T, 4, 4, 4, ref rowsc, ref colsc); effector.drawEffector(gr, xdim, ydim); // link #1 cylinder Cylinder cylinder1 = new Cylinder(4.5, 4.5, 10, -3, 0, Color.Blue, Cylinder.ALONGY); cylinder1.T = links[0].T; cylinder1.drawCylinder(gr, xdim, ydim); // link #4 cylinder Cylinder cylinder4 = new Cylinder(R, 4.5, 10, -3, 0, Color.Peru, Cylinder.ALONGX); cylinder4.T = ArmLink.mulMatrices(links[3].T, links[4].T, 4, 4, 4, ref rowsc, ref colsc); cylinder4.T = ArmLink.mulMatrices(links[2].T, cylinder4.T, 4, 4, 4, ref rowsc, ref colsc); cylinder4.T = ArmLink.mulMatrices(links[1].T, cylinder4.T, 4, 4, 4, ref rowsc, ref colsc); cylinder4.drawCylinder(gr, xdim, ydim); // link #3 cylinder Cylinder cylinder3 = new Cylinder(R, 16.5, 10, -3, 0, Color.Green, Cylinder.ALONGX); cylinder3.T = ArmLink.mulMatrices(links[2].T, links[3].T, 4, 4, 4, ref rowsc, ref colsc); cylinder3.T = ArmLink.mulMatrices(links[1].T, cylinder3.T, 4, 4, 4, ref rowsc, ref colsc); cylinder3.drawCylinder(gr, xdim, ydim); // link#2 cylinder Cylinder cylinder2 = new Cylinder(R, 13.5, 10, -3, 0, Color.Red, Cylinder.ALONGX); cylinder2.T = ArmLink.mulMatrices(links[1].T, links[2].T, 4, 4, 4, ref rowsc, ref colsc); cylinder2.drawCylinder(gr, xdim, ydim); }
public ArmLink(ArmLink prevlink, double dispX, double dispY, double dispZ, double rotX, double rotY, double rotZ) { int i, j; ArmLink newlink = ArmLink.translateXYZ(prevlink, dispX, dispY, dispZ); newlink = ArmLink.rotateX(newlink, rotX); newlink = ArmLink.rotateY(newlink, rotY); newlink = ArmLink.rotateZ(newlink, rotY); // copying transfomation matrix from newlink to the object for (i = 0; i < 4; i++) { for (j = 0; j < 4; j++) { this.T[i][j] = newlink.T[i][j]; } } // transformation matrix copied }
public void addDHline(double anglei_1, double ai_1, double di, double homeangle) { linknum++; // creating a new line in the Denavit - Hartenberg matrix DHmatrix[linknum - 1] = new double[4]; // filling the line now DHmatrix[linknum - 1][0] = anglei_1; // angle about the x-axis // bewteen i-1 and i link DHmatrix[linknum - 1][1] = ai_1; // distance between i-1 and i link // on the x-axis DHmatrix[linknum - 1][2] = di; // displace of link i on the z-axis DHmatrix[linknum - 1][3] = anglei_1; // home angle of link-i // Denavit - Hartenberg line added // Now assigning a frame to the link links[linknum - 1] = ArmLink.rotateX(links[linknum - 2], anglei_1); // rotating about x by angle i-1 // to align with link i links[linknum - 1] = ArmLink.translateXYZ(links[linknum - 1], ai_1, 0, di); // translating on x by the distance bewteen // i and i-1 and translating on z by displacement di links[linknum - 1] = ArmLink.rotateZ(links[linknum - 1], homeangle); // rotating about z to home position (angle theta) }
public double[][] linkPosition(int linkindex) { int i, rows = 0, cols = 0; double[][] zeropoint; zeropoint = new double[4][]; for (i = 0; i < 4; i++) { zeropoint[i] = new double[1]; } zeropoint[0][0] = 0; zeropoint[1][0] = 0; zeropoint[2][0] = 0; zeropoint[3][0] = 1; zeropoint = ArmLink.mulMatrices(links[linkindex].T, zeropoint, 4, 4, 1, ref rows, ref cols); for (i = linkindex - 1; i > 0; i--) { zeropoint = ArmLink.mulMatrices(links[i].T, zeropoint, 4, 4, 1, ref rows, ref cols); } return(zeropoint); }
public void drawEffector(Graphics gr, int panelxdim, int panelydim) { double x, y, z; System.Drawing.Pen pen1 = new Pen(Color.Aqua); System.Drawing.Pen pen2 = new Pen(Color.Brown); double[][] point1 = new double[4][]; double[][] point2 = new double[4][]; double[][] point3 = new double[4][]; double[][] point4 = new double[4][]; int i; for (i = 0; i < 4; i++) { point1[i] = new double[1]; point2[i] = new double[1]; point3[i] = new double[1]; point4[i] = new double[1]; } for (y = -height / 2; y <= height / 2; y += 0.05) { point1[0][0] = 0; point1[1][0] = y; point1[2][0] = opening * width / 200; point1[3][0] = 1; point2[0][0] = fingerlen; point2[1][0] = y; point2[2][0] = opening * width / 200; point2[3][0] = 1; double x1, y1, z1; double x2, y2, z2; point3[0][0] = 0; point3[1][0] = y; point3[2][0] = -opening * width / 200; point3[3][0] = 1; point4[0][0] = fingerlen; point4[1][0] = y; point4[2][0] = -opening * width / 200; point4[3][0] = 1; double x3 = 0, y3, z3; double x4, y4, z4; // transforming points using transformation matrix T int rows = 0, cols = 0; point1 = ArmLink.mulMatrices(T, point1, 4, 4, 1, ref rows, ref cols); point2 = ArmLink.mulMatrices(T, point2, 4, 4, 1, ref rows, ref cols); point3 = ArmLink.mulMatrices(T, point3, 4, 4, 1, ref rows, ref cols); point4 = ArmLink.mulMatrices(T, point4, 4, 4, 1, ref rows, ref cols); x1 = point1[0][0]; y1 = point1[1][0]; z1 = point1[2][0]; x2 = point2[0][0]; y2 = point2[1][0]; z2 = point2[2][0]; x3 = point3[0][0]; y3 = point3[1][0]; z3 = point3[2][0]; x4 = point4[0][0]; y4 = point4[1][0]; z4 = point4[2][0]; // converting coordinates to 2D double Sx1 = ((x1 - eyeofsX) * eyedistance / (z1 + eyedistance)); double Sy1 = ((y1 - eyeofsY) * eyedistance / (z1 + eyedistance)); double Sx2 = ((x2 - eyeofsX) * eyedistance / (z2 + eyedistance)); double Sy2 = ((y2 - eyeofsY) * eyedistance / (z2 + eyedistance)); double Sx3 = ((x3 - eyeofsX) * eyedistance / (z3 + eyedistance)); double Sy3 = ((y3 - eyeofsY) * eyedistance / (z3 + eyedistance)); double Sx4 = ((x4 - eyeofsX) * eyedistance / (z4 + eyedistance)); double Sy4 = ((y4 - eyeofsY) * eyedistance / (z4 + eyedistance)); double pixpercmX = panelxdim / 35; double pixpercmY = panelydim / 35; // drawing lines gr.DrawLine(pen2, (float)(Sx1 * pixpercmX), (float)(panelydim - Sy1 * pixpercmY), (float)(Sx2 * pixpercmX), (float)(panelydim - Sy2 * pixpercmY)); gr.DrawLine(pen2, (float)(Sx3 * pixpercmX), (float)(panelydim - Sy3 * pixpercmY), (float)(Sx4 * pixpercmX), (float)(panelydim - Sy4 * pixpercmY)); } for (y = -height / 2; y <= height / 2; y += 0.05) { point1[0][0] = 0; point1[1][0] = y; point1[2][0] = -width / 2; point1[3][0] = 1; point2[0][0] = 0; point2[1][0] = y; point2[2][0] = width / 2; point2[3][0] = 1; double x1, y1, z1; double x2, y2, z2; // transforming points using transformation matrix T int rows = 0, cols = 0; point1 = ArmLink.mulMatrices(T, point1, 4, 4, 1, ref rows, ref cols); point2 = ArmLink.mulMatrices(T, point2, 4, 4, 1, ref rows, ref cols); x1 = point1[0][0]; y1 = point1[1][0]; z1 = point1[2][0]; x2 = point2[0][0]; y2 = point2[1][0]; z2 = point2[2][0]; // converting coordinates to 2D double Sx1 = ((x1 - eyeofsX) * eyedistance / (z1 + eyedistance)); double Sy1 = ((y1 - eyeofsY) * eyedistance / (z1 + eyedistance)); double Sx2 = ((x2 - eyeofsX) * eyedistance / (z2 + eyedistance)); double Sy2 = ((y2 - eyeofsY) * eyedistance / (z2 + eyedistance)); double pixpercmX = panelxdim / 35; double pixpercmY = panelydim / 35; // drawing lines gr.DrawLine(pen1, (float)(Sx1 * pixpercmX), (float)(panelydim - Sy1 * pixpercmY), (float)(Sx2 * pixpercmX), (float)(panelydim - Sy2 * pixpercmY)); } pen1.Dispose(); pen2.Dispose(); }
public void drawCylinder(Graphics gr, int panelxdim, int panelydim) { int i; double[][] basecenter = new double[4][]; for (i = 0; i < 4; i++) { basecenter[i] = new double[1]; } double[][] peekcenter = new double[4][]; for (i = 0; i < 4; i++) { peekcenter[i] = new double[1]; } double[][] baseperim = new double[4][]; for (i = 0; i < 4; i++) { baseperim[i] = new double[1]; } double[][] peekperim = new double[4][]; for (i = 0; i < 4; i++) { peekperim[i] = new double[1]; } // constructing base center point basecenter[0][0] = 0; basecenter[1][0] = 0; basecenter[2][0] = 0; basecenter[3][0] = 1; switch (direction) { case ALONGX: // constructing peek center point peekcenter[0][0] = H; peekcenter[1][0] = 0; peekcenter[2][0] = 0; peekcenter[3][0] = 1; break; case ALONGY: // constructing peek center point peekcenter[0][0] = 0; peekcenter[1][0] = H; peekcenter[2][0] = 0; peekcenter[3][0] = 1; break; case ALONGZ: // constructing peek center point peekcenter[0][0] = 0; peekcenter[1][0] = H; peekcenter[2][0] = 0; peekcenter[3][0] = 1; break; } for (i = 0; i < 360; i++) { switch (direction) { case ALONGX: // base circumference point baseperim[0][0] = 0; baseperim[1][0] = R * Math.Cos(Math.PI * i / 180); baseperim[2][0] = R * Math.Sin(Math.PI * i / 180); baseperim[3][0] = 1; // peek base circumference point peekperim[0][0] = H; peekperim[1][0] = R * Math.Cos(Math.PI * i / 180); peekperim[2][0] = R * Math.Sin(Math.PI * i / 180); peekperim[3][0] = 1; break; case ALONGY: // base circumference point baseperim[0][0] = R * Math.Cos(Math.PI * i / 180); baseperim[1][0] = 0; baseperim[2][0] = R * Math.Sin(Math.PI * i / 180); baseperim[3][0] = 1; // peek base circumference point peekperim[0][0] = R * Math.Cos(Math.PI * i / 180); peekperim[1][0] = H; peekperim[2][0] = R * Math.Sin(Math.PI * i / 180); peekperim[3][0] = 1; break; case ALONGZ: // base circumference point baseperim[0][0] = R * Math.Cos(Math.PI * i / 180); baseperim[1][0] = R * Math.Sin(Math.PI * i / 180); baseperim[2][0] = 0; baseperim[3][0] = 1; // peek base circumference point peekperim[0][0] = R * Math.Cos(Math.PI * i / 180); peekperim[1][0] = R * Math.Sin(Math.PI * i / 180); peekperim[2][0] = H; peekperim[3][0] = 1; break; } // transforming coordinates with translation matrix int rows = 0, cols = 0; baseperim = ArmLink.mulMatrices(T, baseperim, 4, 4, 1, ref rows, ref cols); peekperim = ArmLink.mulMatrices(T, peekperim, 4, 4, 1, ref rows, ref cols); // Computing screen coordinates double Xs1, Ys1, Xs2, Ys2, x1, y1, z1, x2, y2, z2; x1 = baseperim[0][0]; y1 = baseperim[1][0]; z1 = baseperim[2][0]; x2 = peekperim[0][0]; y2 = peekperim[1][0]; z2 = peekperim[2][0]; Ys1 = ((y1 - eyeofsY) * eyedistance / (z1 + eyedistance)); Xs1 = ((x1 - eyeofsX) * eyedistance / (z1 + eyedistance)); Ys2 = (y2 - eyeofsY) * eyedistance / (z2 + eyedistance); Xs2 = (x2 - eyeofsX) * eyedistance / (z2 + eyedistance); double pixpercmX = panelxdim / 35; double pixpercmY = panelydim / 35; System.Drawing.Pen pen = new Pen(color); // drawing circles gr.DrawEllipse(pen, (float)(Xs1 * pixpercmX), (float)(panelydim - Ys1 * pixpercmY), 1, 1); gr.DrawEllipse(pen, (float)(Xs2 * pixpercmX), (float)(panelydim - Ys2 * pixpercmY), 1, 1); if (i % 15 == 0) { gr.DrawLine(pen, (float)(Xs1 * pixpercmX), (float)(panelydim - Ys1 * pixpercmY), (float)(Xs2 * pixpercmX), (float)(panelydim - Ys2 * pixpercmY)); } } }
private void timer1_Tick(object sender, EventArgs e) { clockticks++; if (clockticks <= trajectory.len) { if (trajectory.moves[clockticks - 1].Dbase != 0) { arm.links[1] = ArmLink.rotateY(arm.links[1], trajectory.moves[clockticks - 1].Dbase); } if (trajectory.moves[clockticks - 1].Dth1 != 0) { arm.links[2] = ArmLink.rotateZ(arm.links[2], trajectory.moves[clockticks - 1].Dth1); } if (trajectory.moves[clockticks - 1].Dth2 != 0) { arm.links[3] = ArmLink.rotateZ(arm.links[3], trajectory.moves[clockticks - 1].Dth2); } if (trajectory.moves[clockticks - 1].Dth3 != 0) { arm.links[4] = ArmLink.rotateZ(arm.links[4], trajectory.moves[clockticks - 1].Dth3); } g.Clear(Color.White); arm.drawArmcyl(xdim, ydim, g); double[][] effector = arm.linkPosition(6); Link1AngleText.Text = Convert.ToString(arm.links[1].angley * 180 / Math.PI); Link2AngleText.Text = Convert.ToString(arm.links[2].anglez * 180 / Math.PI); Link3AngleText.Text = Convert.ToString(arm.links[3].anglez * 180 / Math.PI); Link4AngleText.Text = Convert.ToString(arm.links[4].anglez * 180 / Math.PI); double x = effector[0][0]; double y = effector[1][0]; double z = effector[2][0]; String str = Convert.ToString(x) + " , " + Convert.ToString(y) + " , " + Convert.ToString(z); effectorPositionText.Text = str; EulerParams orientation = arm.linkAngles(6); yawText.Text = Convert.ToString(orientation.Yaw * 180 / Math.PI); pitchText.Text = Convert.ToString(orientation.Pitch * 180 / Math.PI); rollText.Text = Convert.ToString(orientation.Roll * 180 / Math.PI); if (alligned) { String cmd; // link#1 motion command if (Servo0BaseIndexRadio.Checked) { cmd = " #0 P"; } else { cmd = " #1 P"; } int basepos = RobotArm.baseServo(arm.links[1].angley); cmd += Convert.ToString(basepos); cmd += " T1000\r"; serialPort1.Write(cmd); // link #2 motion command if (Servo0BaseIndexRadio.Checked) { cmd = " #1 P"; } else { cmd = " #2 P"; } int link2pos = RobotArm.link2Servo(arm.links[2].anglez); cmd += Convert.ToString(link2pos); cmd += " T1000\r"; serialPort1.Write(cmd); // link #3 motion command if (Servo0BaseIndexRadio.Checked) { cmd = "#2 P"; } else { cmd = " #3 P"; } int link3pos = RobotArm.link3Servo(arm.links[3].anglez); cmd += Convert.ToString(link3pos); cmd += " T1000\r"; serialPort1.Write(cmd); // link #4 motion command if (Servo0BaseIndexRadio.Checked) { cmd = " #3 P"; } else { cmd = " #4 P"; } int link4pos = RobotArm.link4Servo(arm.links[4].anglez); cmd += Convert.ToString(link4pos); cmd += " T1000\r"; serialPort1.Write(cmd); } } else { moving = false; timer1.Enabled = false; } }