//定义机器人
//*********注意：：各个臂的位置因是其旋转中心点的位置，否则计算会出错！***********
public void DefineRobot(string[] names)
{
if (names.Length < 6)
{
Debug.LogError("请输入机器人六个臂的名称");
return;
}
for (int i = 0; i < 6; i++)
{
joints[i] = new JointMove(names[i], joint_directs[i]);
joints[i].SoftLimit = soft_limits[i * 2];
joints[i]._SoftLimit = soft_limits[i * 2 + 1];
}
auto_calculate(names);
IKA = new InverseKinematicsAlgorithm(a1, a2, a3, d4);
}
//定义机器人
public void DefineJMove(string name)
{
switch (name)
{
case "case1":
case "case2":
IKA = new InverseKinematicsAlgorithm(GSKDATA.ROBOT1_a1, GSKDATA.ROBOT1_a2, GSKDATA.ROBOT1_a3, GSKDATA.ROBOT1_d4);
J1 = GameObject.Find("MechanicalArm_11");
J2 = GameObject.Find("MechanicalArm_21");
J3 = GameObject.Find("MechanicalArm_31");
J4 = GameObject.Find("MechanicalArm_41");
J5 = GameObject.Find("MechanicalArm_51");
J6 = GameObject.Find("MechanicalArm_61");
JMove_1 = new JointMove(J1, new Vector3(0, 1, 0));
JMove_2 = new JointMove(J2, new Vector3(0, 0, 1));
JMove_3 = new JointMove(J3, new Vector3(0, 0, 1));
JMove_4 = new JointMove(J4, new Vector3(-1, 0, 0));
JMove_5 = new JointMove(J5, new Vector3(0, 0, 1));
JMove_6 = new JointMove(J6, new Vector3(-1, 0, 0));
//设置软极限
JMove_1.SoftLimit = 180;
JMove_1._SoftLimit = -180;
JMove_2.SoftLimit = 120;
JMove_2._SoftLimit = -80;
JMove_3.SoftLimit = 80;
JMove_3._SoftLimit = -120;
JMove_4.SoftLimit = 170;
JMove_4._SoftLimit = -170;
JMove_5.SoftLimit = 130;
JMove_5._SoftLimit = -130;
JMove_6.SoftLimit = 360;
JMove_6._SoftLimit = -360;
AxisPositionSet(0f, 0f, 0, 0, 0, 0);
break;
case "case3":
IKA = new InverseKinematicsAlgorithm(GSKDATA.ROBOT1_A1, GSKDATA.ROBOT1_A2, GSKDATA.ROBOT1_A3, GSKDATA.ROBOT1_D4);
J1 = GameObject.Find("MechanicalArm_13");
J2 = GameObject.Find("MechanicalArm_23");
J3 = GameObject.Find("MechanicalArm_33");
J4 = GameObject.Find("MechanicalArm_43");
J5 = GameObject.Find("MechanicalArm_53");
J6 = GameObject.Find("MechanicalArm_63");
JMove_1 = new JointMove(J1, new Vector3(0, 1, 0));
JMove_2 = new JointMove(J2, new Vector3(0, 0, 1));
JMove_3 = new JointMove(J3, new Vector3(0, 0, 1));
JMove_4 = new JointMove(J4, new Vector3(-1, 0, 0));
JMove_5 = new JointMove(J5, new Vector3(0, 0, 1));
JMove_6 = new JointMove(J6, new Vector3(-1, 0, 0));
//设置软极限
JMove_1.SoftLimit = 180;
JMove_1._SoftLimit = -180;
JMove_2.SoftLimit = 120;
JMove_2._SoftLimit = -80;
JMove_3.SoftLimit = 80;
JMove_3._SoftLimit = -120;
JMove_4.SoftLimit = 170;
JMove_4._SoftLimit = -170;
JMove_5.SoftLimit = 130;
JMove_5._SoftLimit = -130;
JMove_6.SoftLimit = 360;
JMove_6._SoftLimit = -360;
AxisPositionSet(0f, -17f, 17, 0, 65, 0);
break;
case "case4":
IKA = new InverseKinematicsAlgorithm(GSKDATA.ROBOT4_A1, GSKDATA.ROBOT4_A2, GSKDATA.ROBOT4_A3, GSKDATA.ROBOT4_D4);
J1 = GameObject.Find("MechanicalArm_14");
J2 = GameObject.Find("MechanicalArm_24");
J3 = GameObject.Find("MechanicalArm_34");
J4 = GameObject.Find("MechanicalArm_44");
J5 = GameObject.Find("MechanicalArm_54");
J6 = GameObject.Find("MechanicalArm_64");
JMove_1 = new JointMove(J1, new Vector3(0, 1, 0));
JMove_2 = new JointMove(J2, new Vector3(0, 0, 1));
JMove_3 = new JointMove(J3, new Vector3(0, 0, 1));
JMove_4 = new JointMove(J4, new Vector3(-1, 0, 0));
JMove_5 = new JointMove(J5, new Vector3(0, 0, 1));
JMove_6 = new JointMove(J6, new Vector3(-1, 0, 0));
//设置软极限
JMove_1.SoftLimit = 180;
JMove_1._SoftLimit = -180;
JMove_2.SoftLimit = 120;
JMove_2._SoftLimit = -80;
JMove_3.SoftLimit = 80;
JMove_3._SoftLimit = -120;
JMove_4.SoftLimit = 170;
JMove_4._SoftLimit = -170;
JMove_5.SoftLimit = 130;
JMove_5._SoftLimit = -130;
JMove_6.SoftLimit = 360;
JMove_6._SoftLimit = -360;
AxisPositionSet(0f, -35f, 37, 0, 35, 0);
break;
case "case6":
IKA = new InverseKinematicsAlgorithm(GSKDATA.ROBOT1_A1, GSKDATA.ROBOT1_A2, GSKDATA.ROBOT1_A3, GSKDATA.ROBOT1_D4);
J1 = GameObject.Find("MechanicalArm_13");
J2 = GameObject.Find("MechanicalArm_23");
J3 = GameObject.Find("MechanicalArm_33");
J4 = GameObject.Find("MechanicalArm_43");
J5 = GameObject.Find("MechanicalArm_53");
J6 = GameObject.Find("MechanicalArm_63_2");
JMove_1 = new JointMove(J1, new Vector3(0, 1, 0));
JMove_2 = new JointMove(J2, new Vector3(0, 0, 1));
JMove_3 = new JointMove(J3, new Vector3(0, 0, 1));
JMove_4 = new JointMove(J4, new Vector3(-1, 0, 0));
JMove_5 = new JointMove(J5, new Vector3(0, 0, 1));
JMove_6 = new JointMove(J6, new Vector3(-1, 0, 0));
//设置软极限
JMove_1.SoftLimit = 180;
JMove_1._SoftLimit = -180;
JMove_2.SoftLimit = 120;
JMove_2._SoftLimit = -80;
JMove_3.SoftLimit = 80;
JMove_3._SoftLimit = -120;
JMove_4.SoftLimit = 170;
JMove_4._SoftLimit = -170;
JMove_5.SoftLimit = 130;
JMove_5._SoftLimit = -130;
JMove_6.SoftLimit = 360;
JMove_6._SoftLimit = -360;
AxisPositionSet(160f, 0f, 0, 0, 90, 0);
break;
case "case5":
IKA = new InverseKinematicsAlgorithm(GSKDATA.ROBOT1_A1, GSKDATA.ROBOT1_A2, GSKDATA.ROBOT1_A3, GSKDATA.ROBOT1_D4);
J1 = GameObject.Find("MechanicalArm_16");
J2 = GameObject.Find("MechanicalArm_26");
J3 = GameObject.Find("MechanicalArm_36");
J4 = GameObject.Find("MechanicalArm_46");
J5 = GameObject.Find("MechanicalArm_56");
J6 = GameObject.Find("MechanicalArm_66");
robot = GameObject.Find("robot6");
JMove_1 = new JointMove(J1, new Vector3(0, 1, 0));
JMove_2 = new JointMove(J2, new Vector3(1, 0, 0));
JMove_3 = new JointMove(J3, new Vector3(1, 0, 0));
JMove_4 = new JointMove(J4, new Vector3(0, 0, -1));
JMove_5 = new JointMove(J5, new Vector3(1, 0, 0));
JMove_6 = new JointMove(J6, new Vector3(0, 0, -1));
//设置软极限
JMove_1.SoftLimit = 180;
JMove_1._SoftLimit = -180;
JMove_2.SoftLimit = 120;
JMove_2._SoftLimit = -80;
JMove_3.SoftLimit = 80;
JMove_3._SoftLimit = -120;
JMove_4.SoftLimit = 170;
JMove_4._SoftLimit = -170;
JMove_5.SoftLimit = 130;
JMove_5._SoftLimit = -130;
JMove_6.SoftLimit = 360;
JMove_6._SoftLimit = -360;
AxisPositionSet(0f, -46f, 45f, 0f, 30f, 0f);
break;
}
}
