// public float dtheta_dt0,dtheta_dt1; // Start is called before the first frame update void Start() { scale_fact = 0.5f; // Robot1=new DeltaRobot(scale_fact*4.773502f,scale_fact*10f,scale_fact*21.96040974f,scale_fact*3.58013f); Robot1 = new DeltaRobot(2f, 5f, 9f, 1f); Robot1.assign_end_point_pisition(scale_fact * new Vector3(0, -24f, 0)); time = 0f; //inv_jaco= Robot1.differential_inverse_kinematics(end_point); Robot1.dtheta_dt_l = new float[] { 0, 0, 0 }; // float[] joint_angle_l= {(Mathf.PI)/3f,(Mathf.PI)/4f,(Mathf.PI)/5f} ; // Vector3[] frwd_jaco=Robot1.differential_forward_kinematics(joint_angle_l,true); }
// Start is called before the first frame update void Start() { //initialise the parameters unified_motor_force = 300f; //Find the framework GameObject CGA_Library_capsule = GameObject.Find("CGA Library"); DeltaRobotClass DeltaRobotFile = CGA_Library_capsule.GetComponent <DeltaRobotClass> (); theRobot = DeltaRobotFile.Robot1; //Find the upper arm models. UpperArms = transform.GetChild(3).gameObject; UpperArms_l = new GameObject[3]; UpperArmRigbody_l = new Rigidbody[3]; UpperArmHingeJointList = new HingeJoint[3]; for (int i = 0; i < 3; i++) { UpperArms_l[i] = UpperArms.transform.GetChild(i).gameObject; UpperArmRigbody_l[i] = UpperArms_l[i].GetComponent <Rigidbody>(); UpperArmHingeJointList[i] = UpperArmRigbody_l[i].GetComponents <HingeJoint>()[0]; } }
// Update is called once per frame void FixedUpdate() { GameObject CGA_Library_capsule = GameObject.Find("CGA Library"); DeltaRobotClass DeltaRobotFile = CGA_Library_capsule.GetComponent <DeltaRobotClass> (); DeltaRobot theRobot = DeltaRobotFile.Robot1; currentposition = (1f / 3f) * (Platform_D.transform.position + Platform_F.transform.position + Platform_E.transform.position - Base_A.transform.position - Base_B.transform.position - Base_C.transform.position); end_point_error = theRobot.end_point - currentposition; if (use_spring == true && use_motor == false) { GameObject CGA_Model_Manager = GameObject.Find("CGA Model Manager"); ArmDemo2 ArmFrame = CGA_Model_Manager.GetComponent <ArmDemo2> (); float Angle_A = ArmFrame.Angle_A; JointSpring sprA = hingeA.spring; sprA.targetPosition = -Angle_A + 36.87f; sprA.spring = springConst; sprA.damper = damperConst; hingeA.spring = sprA; hingeA.useSpring = use_spring; float Angle_B = ArmFrame.Angle_B; JointSpring sprB = hingeB.spring; sprB.targetPosition = -Angle_B + 36.87f; sprB.spring = springConst; sprB.damper = damperConst; hingeB.spring = sprB; hingeB.useSpring = use_spring; float Angle_C = ArmFrame.Angle_C; JointSpring sprC = hingeC.spring; sprC.targetPosition = -Angle_C + 36.87f; sprC.spring = springConst; sprC.damper = damperConst; hingeC.spring = sprC; hingeC.useSpring = use_spring; } else if (use_spring == false && use_motor == true) { float[] dtheta_dt_l = theRobot.dtheta_dt_l; // float[] dtheta_dt_l=new float[]{0,0,0}; float k_p0 = 700f; float k_p = 70f; float k_d = 0f, k_i = 0f; Vector3 derivative_error = end_point_error - last_error; accum_error += end_point_error; Vector3 input_velocity = k_p * end_point_error + k_d * derivative_error + k_i * accum_error; // Vector3 input_velocity= k_p*end_point_error; float [,] inv_jacob = theRobot.differential_inverse_kinematics(); // for(int i=0;i<3;i++){ // dtheta_dt_l[i]=inv_jacob[i,0]*input_velocity.x+inv_jacob[i,1]*input_velocity.y+inv_jacob[i,2]*input_velocity.z; // } HingeMoterVelocityA = k_p0 * dtheta_dt_l[0]; hingeA.useSpring = false; hingeA.useMotor = true; JointMotor temp_motor = hingeA.motor; temp_motor.force = unified_motor_force; temp_motor.targetVelocity = HingeMoterVelocityA; temp_motor.freeSpin = true; hingeA.motor = temp_motor; HingeMoterVelocityB = k_p0 * dtheta_dt_l[1]; // ConfigHingeJoint_usingMotor(hingeB, HingeMoterVelocityB, unified_motor_force); hingeB.useSpring = false; hingeB.useMotor = true; temp_motor = hingeB.motor; temp_motor.force = unified_motor_force; temp_motor.targetVelocity = HingeMoterVelocityB; temp_motor.freeSpin = true; hingeB.motor = temp_motor; HingeMoterVelocityC = k_p0 * dtheta_dt_l[2]; // ConfigHingeJoint_usingMotor(hingeC, HingeMoterVelocityC, unified_motor_force); hingeC.useSpring = false; hingeC.useMotor = true; temp_motor = hingeC.motor; temp_motor.force = unified_motor_force; temp_motor.targetVelocity = HingeMoterVelocityC; temp_motor.freeSpin = true; hingeC.motor = temp_motor; } if (use_limit == true) { foreach (HingeJoint HingeX in UpperArmHingeJointList) { ConfigHingeJoint_limit(HingeX, limit_min, limit_max, limit_bounciness, limit_bounceMinVelocity); } } updateRigidBodyParam(UpperArmRBList, upper_arm_mass, drag, angular_drag, use_gravity); updateRigidBodyParam(ElbowJointList, elbow_mass, drag, angular_drag, use_gravity); updateRigidBodyParam(LowerArmList, lower_arm_mass, drag, angular_drag, use_gravity); updateRigidBodyParam(PlatformRBList, platform_mass, drag, angular_drag, use_gravity); last_error = end_point_error; }