// 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;
}
