//inits poses of robot arm for drilling holes private void InitDrillingPoses() { List <PoseStampedMsg> poses = programItem.GetPose(); PoseMsg position1 = poses[0].GetPose(); PoseMsg position2 = poses[1].GetPose(); //robots gripper local position starts at the grippers joint (not the tip).. so place the joint position as initial position for tip before drilling pose1_init = new Vector3(position1.GetPosition().GetX(), -position1.GetPosition().GetY(), position1.GetPosition().GetZ()); pose2_init = new Vector3(position2.GetPosition().GetX(), -position2.GetPosition().GetY(), position2.GetPosition().GetZ()); //drilling_hole_1_init = true; drilling_State = drilling_state.INIT_DRILLING_HOLE_1; }
// Update is called once per frame void Update() { if (run) { //if user says "Next" then skip to end state if (next) { SkipToEnd(); run = false; next = false; } //if user says "Previous" then skip to initial state else if (previous) { GoBackToStart(); run = false; previous = false; } //normal run else { if (!drilled) { runTime += Time.deltaTime; //move arm to starting position .. in case that it somewhere already exists if (!arm_in_starting_position) { if (ObjectInPosition(pr2_arm, gripper_init_pose)) { arm_in_starting_position = true; } pr2_arm.transform.localPosition = Vector3.Lerp(pr2_arm.transform.localPosition, gripper_init_pose, Time.deltaTime * 2f); //Look at cube .. arm_rotation - transforms arm to point to object with -x axis forward pr2_arm.transform.localRotation = Quaternion.Slerp(pr2_arm.transform.localRotation, Quaternion.LookRotation(gripper_init_pose - new Vector3(0, 0, 0.3f) - pr2_arm.transform.localPosition) * arm_rotation, Time.deltaTime * 2f); } //start drilling else { //if there is still something to drill if (objectsToDrill.Count > 0) { pr2_animator.SetBool("close_gripper", true); //if gripper is closed, move it down to objectgripper_init_pose if (pr2_animator.GetCurrentAnimatorStateInfo(0).IsName("closed")) { pr2_animator.SetBool("close_gripper", false); //make the arm child of the object and adjust init poses for gripper if (pr2_arm.transform.parent != objectsToDrill[0].transform) { pr2_arm.transform.parent = objectsToDrill[0].transform; AdjustInitPoses(objectsToDrill[0].transform); } //drilling moves switch (drilling_State) { //init drilling first hole case drilling_state.INIT_DRILLING_HOLE_1: pr2_arm.transform.localPosition = Vector3.Lerp(pr2_arm.transform.localPosition, pose1_init, Time.deltaTime * 2f); //pr2_arm.transform.localRotation = Quaternion.Lerp(pr2_arm.transform.localRotation, orientation1, Time.deltaTime * 2f); if (ObjectInPosition(pr2_arm, pose1_init)) { drilling_State = drilling_state.DRILL_HOLE_1; move_down = true; move_up = false; } break; case drilling_state.DRILL_HOLE_1: if (move_down) { pr2_arm_child.transform.localPosition = Vector3.Lerp(pr2_arm_child.transform.localPosition, poseDown, Time.deltaTime * 2f); if (ObjectInPosition(pr2_arm_child, poseDown)) { move_down = false; move_up = true; } } if (move_up) { pr2_arm_child.transform.localPosition = Vector3.Lerp(pr2_arm_child.transform.localPosition, poseUp, Time.deltaTime * 2f); if (ObjectInPosition(pr2_arm_child, poseUp)) { //move the arm to its real previous position (Lerp makes the position slightly inaccurate) pr2_arm_child.transform.localPosition = poseUp; move_down = true; move_up = false; //arm is up .. drilling of hole done .. move to next hole drilling_State = drilling_state.INIT_DRILLING_HOLE_2; } } break; case drilling_state.INIT_DRILLING_HOLE_2: pr2_arm.transform.localPosition = Vector3.Lerp(pr2_arm.transform.localPosition, pose2_init, Time.deltaTime * 2f); //pr2_arm.transform.localRotation = Quaternion.Lerp(pr2_arm.transform.localRotation, orientation2, Time.deltaTime * 2f); if (ObjectInPosition(pr2_arm, pose2_init)) { drilling_State = drilling_state.DRILL_HOLE_2; move_down = true; move_up = false; } break; case drilling_state.DRILL_HOLE_2: if (move_down) { pr2_arm_child.transform.localPosition = Vector3.Lerp(pr2_arm_child.transform.localPosition, poseDown, Time.deltaTime * 2f); if (ObjectInPosition(pr2_arm_child, poseDown)) { move_down = false; move_up = true; } } if (move_up) { pr2_arm_child.transform.localPosition = Vector3.Lerp(pr2_arm_child.transform.localPosition, poseUp, Time.deltaTime * 2f); if (ObjectInPosition(pr2_arm_child, poseUp)) { //move the arm to its real previous position (Lerp makes the position slightly inaccurate) pr2_arm_child.transform.localPosition = poseUp; move_down = true; move_up = false; //init = true; //prepare drilling state for another object drilling_State = drilling_state.INIT_DRILLING_HOLE_1; //unchild the arm after drilling of object is finished pr2_arm.transform.parent = world_anchor.transform; //remove drilled object from list objectsToDrill.RemoveAt(0); //create reference to next object for init pose if (objectsToDrill.Count > 0) { gripper_init_pose = objectsToDrill[0].transform.localPosition + new Vector3(0, 0, 0.3f); } //move back to initial position arm_in_starting_position = false; } } break; } } } //if everything has been drilled then move back to init position and end else { pr2_arm.transform.localPosition = Vector3.Lerp(pr2_arm.transform.localPosition, gripper_init_pose, Time.deltaTime * 2f); if (ObjectInPosition(pr2_arm, gripper_init_pose)) { drilled = true; } } } } else { run = false; } } } }