public void DrawPassiveArm(ref OpenGL gl) { //gl.PushMatrix(); //gl.Translate(-0.5f, 0, -0.5f); //gl.Scale(1f, fPassiveLeg * fRatio, 1f); //gl.Color(0f, 1.0f, 0.5f);//color arm //rc.DrawUnitCylinder(ref gl); // gl.PushMatrix(); InverseKinematic IK = new InverseKinematic(); vec3[] Top = IK.kinv_TopPos(GoalPos.x / fRatio, GoalPos.y / fRatio, GoalPos.z / fRatio); vec3[] Bottom = IK.kinv_BotPos(GoalPos.x / fRatio, GoalPos.y / fRatio, GoalPos.z / fRatio); gl.Color(0f, 1.0f, 0.5f);//color arm //rc.DrawUnitCylinder(ref gl, fRatio * new vec3(Top[0].y, Top[0].x, Top[0].z), fRatio * new vec3(Bottom[0].y, -Bottom[0].x, Bottom[0].z)); rc.DrawUnitCylinder(ref gl, fRatio * new vec3(Top[0].y, Top[0].x, Top[0].z), fRatio * new vec3(Bottom[0].y, -Bottom[0].x, Bottom[0].z)); //gl.Translate(0.5f, 0, 0.5f); // moi sua chieu ngay 31/10 //rc.DrawUnitCylinder(ref gl, fRatio * new vec3(-Top[1].x, -Top[1].y, Top[1].z), fRatio * new vec3(-Bottom[1].x, -Bottom[1].y, Bottom[1].z)); rc.DrawUnitCylinder(ref gl, fRatio * new vec3(Top[1].y, Top[1].x, Top[1].z), fRatio * new vec3(Bottom[1].y, -Bottom[1].x, Bottom[1].z)); //vec3 test1 = fRatio * new vec3(Top[2].x, Top[2].y, Top[2].z); //vec3 test2 = fRatio * new vec3(Bottom[2].x, Bottom[2].y, Bottom[2].z); vec3 test1 = fRatio * new vec3(Top[2].y, Top[2].x, Top[2].z); vec3 test2 = fRatio * new vec3(Bottom[2].y, -Bottom[2].x, Bottom[2].z); rc.DrawUnitCylinder(ref gl, test1, test2); gl.PopMatrix(); }
public Delta_3_Robot(vec3 GPos) { GoalPos = GPos; /* * GoalPos.x = 0.0f; * GoalPos.y = -75.0f; * GoalPos.z = -380.518f;*/ double[] tmp = new double[] { GoalPos.x, GoalPos.y, GoalPos.z }; vec3[] a = new vec3[3]; vec3[] b = new vec3[3]; InverseKinematic IK = new InverseKinematic(); IK.delta_calcInverse(tmp[0], tmp[1], tmp[2], ref ThetaMotor, ref ThetaPassiveRy, ref ThetaPassiveRz); }
private void openGLControl_OpenGLDraw(object sender, PaintEventArgs e) { OpenGL gl = openGLControl.OpenGL; gl.Clear(OpenGL.GL_COLOR_BUFFER_BIT | OpenGL.GL_DEPTH_BUFFER_BIT); gl.LoadIdentity(); gl.Perspective(fFovy, (double)Width / (double)Height, 0.01, 400.0); gl.LookAt(vDrawOriginal.x, vDrawOriginal.y, vDrawOriginal.z, 0, 0, 0, 0, 1, 0); gl.Rotate(vRotAngle.x, 1.0f, 0.0f, 0.0f); gl.Rotate(vRotAngle.y, 0.0f, 1.0f, 0.0f); gl.Rotate(vRotAngle.z, 0.0f, 0.0f, 1.0f); drawGrid(gl); Delta_3_Robot D3R = new Delta_3_Robot(new vec3(MovePlatePos.x, MovePlatePos.y, MovePlatePos.z)); D3R.DrawDelta3Robot(ref gl); double[] ThetaMotor = new double[3]; double[] ThetaPassiveRx = new double[3]; double[] ThetaPassiveRy = new double[3]; InverseKinematic IK = new InverseKinematic(); IK.delta_calcInverse(MovePlatePos.x, MovePlatePos.y, MovePlatePos.z, ref ThetaMotor, ref ThetaPassiveRx, ref ThetaPassiveRy); /* label6.Text = "Leg1 Theta 1: " + ThetaMotor[0]; * label5.Text = "Leg2 Theta 1: " + ThetaMotor[1]; * label4.Text = "Leg3 Theta 1: " + ThetaMotor[2]; * * label9.Text = "Leg1 Theta 2 Rx: " + ThetaPassiveRx[0]; * label8.Text = "Leg2 Theta 2 Rx: " + ThetaPassiveRx[1]; * label7.Text = "Leg3 Theta 2 Rx: " + ThetaPassiveRx[2]; * * label12.Text = "Leg1 Theta 2 Ry: " + ThetaPassiveRy[0]; * label11.Text = "Leg2 Theta 2 Ry: " + ThetaPassiveRy[1]; * label10.Text = "Leg3 Theta 2 Ry: " + ThetaPassiveRy[2]; */ tbXCoor.Text = Math.Round(MovePlatePos.x, 4).ToString(); tbYCoor.Text = Math.Round(MovePlatePos.y, 4).ToString(); tbZCoor.Text = Math.Round(MovePlatePos.z, 4).ToString(); UpdateCoorRv(); tbθ1Coor.Text = Math.Round(ThetaMotor[0], 4).ToString(); tbθ2Coor.Text = Math.Round(ThetaMotor[1], 4).ToString(); tbθ3Coor.Text = Math.Round(ThetaMotor[2], 4).ToString(); }