public void SetPos(float angle0, float angle1, float angle2, float angle3, float angle4, float angle5, bool gripped) { if (config.Kinematic) { float deltaangle0 = kinematicanglerange0.GetTarget() - chassis.GetComponent <chassismanipulator2>().GetKinematicRestDeltaAngle0(); float deltaangle1 = kinematicanglerange1.GetTarget() - chassis.GetComponent <chassismanipulator2>().GetKinematicRestDeltaAngle1(); float deltaangle2 = kinematicanglerange2.GetTarget() - chassis.GetComponent <chassismanipulator2>().GetKinematicRestDeltaAngle2(); kinematicanglerange0.SetTarget(angle0); kinematicanglerange1.SetTarget(angle1); kinematicanglerange2.SetTarget(-90 + angle2); deltaangle0 = kinematicanglerange0.Delta(deltaangle0, kinematicanglerange0.GetTarget()); deltaangle1 = kinematicanglerange0.Delta(deltaangle1, kinematicanglerange1.GetTarget()); deltaangle2 = kinematicanglerange0.Delta(deltaangle2, kinematicanglerange2.GetTarget()); chassis.GetComponent <chassismanipulator2>().SetKinematicTargetDeltaAngles(deltaangle0, deltaangle1, deltaangle2); } else { DriveJoint drive0 = chassis.GetComponent <chassismanipulator2>().drive; DriveJoint drive1 = lever.GetComponent <levermanipulator2>().drive; DriveJoint drive2 = holder1.GetComponent <holder1manipulator2>().drive; DriveJoint drive3 = holder2.GetComponent <holder2manipulator2>().drive; drive0.AngleRange.SetTarget(angle0); drive3.AngleRange.SetTarget(angle1); drive2.AngleRange.SetTarget(drive3.AngleRange.GetTarget() + drive1.AngleRange.GetTarget()); drive1.AngleRange.SetTarget(-90 + angle2); drive2.AngleRange.SetTarget(drive3.AngleRange.GetTarget() + drive1.AngleRange.GetTarget()); } if (config.FingerDown) { capture.SetPos(0, GetPos1() + GetPos2() - 180, angle5); } else { capture.SetPos(angle3, angle4, angle5); } SetGripper(gripped); }