private void InitPlacePose() { placePose = programItem.GetPose()[0].GetPose(); placePosition = new Vector3(placePose.GetPosition().GetX(), -placePose.GetPosition().GetY(), placePose.GetPosition().GetZ()); //placePosition = new Vector3(placePose.GetPosition().GetX(), -placePose.GetPosition().GetY(), 0f); placeQuaternion = new Quaternion(-placePose.GetOrientation().GetX(), placePose.GetOrientation().GetY(), -placePose.GetOrientation().GetZ(), placePose.GetOrientation().GetW()); upPosition = objectToPlace.transform.localPosition + new Vector3(0, 0, 0.1f); }
//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; }
private static bool PoseSet(PoseMsg pose) { if (pose.GetPosition().GetX() == 0f && pose.GetPosition().GetY() == 0f && pose.GetPosition().GetZ() == 0f && pose.GetOrientation().GetX() == 0f && pose.GetOrientation().GetY() == 0f && pose.GetOrientation().GetZ() == 0f && pose.GetOrientation().GetW() == 0f) { return(false); } return(true); }
//called when restart void OnEnable() { if (objectToPick != null && programItem != null) { objectToPick.SetActive(true); PoseMsg gripper_pose = programItem.GetPose()[0].GetPose(); objectToPick.transform.localPosition = new Vector3(gripper_pose.GetPosition().GetX(), -gripper_pose.GetPosition().GetY(), gripper_pose.GetPosition().GetZ()); if (left_feeder) { //rotate the object to make right move objectToPick.transform.localEulerAngles = new Vector3(-90f, 90f, 0f); //position the object to fit approximately on the feeder objectToPick.transform.localPosition += new Vector3(-0.30f, 0f, 0f); } else { //rotate the object to make right move objectToPick.transform.localEulerAngles = new Vector3(-90f, -90f, 0f); //position the object to fit approximately on the feeder objectToPick.transform.localPosition += new Vector3(0.30f, 0f, 0f); } //InitRobotGripper(); PlaceRobotGripperToInit(); } }
// 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 (!movedToPose) { runTime += Time.deltaTime; if (objectToMove != null) { //grab the object and move it up if (moving_up) { objectToMove.transform.localPosition = Vector3.Lerp(objectToMove.transform.localPosition, upPosition, Time.deltaTime * 2f); if (ObjectInPosition(objectToMove, upPosition)) { moving_up = false; moving_to_place = true; } } //move the object above the place position if (moving_to_place) { objectToMove.transform.localPosition = Vector3.Lerp(objectToMove.transform.localPosition, new Vector3(placePose.GetPosition().GetX(), -placePose.GetPosition().GetY(), placePose.GetPosition().GetZ()), Time.deltaTime * 2f); objectToMove.transform.localRotation = Quaternion.Lerp(objectToMove.transform.localRotation, placeQuaternion, Time.deltaTime * 2f); if (ObjectInPosition(objectToMove, new Vector3(placePose.GetPosition().GetX(), -placePose.GetPosition().GetY(), placePose.GetPosition().GetZ()))) { moving_to_place = false; //moving_down = true; movedToPose = true; } } } } else { run = false; } } } }
private void InitRobotGripper() { //make sure that the robots arm is set to be the child of world_anchor pr2_arm.transform.parent = world_anchor.transform; pr2_arm.transform.GetChild(0).gameObject.SetActive(true); //determine from which feeder the robot is going to grab.. if it's on the left side of the table (<1m) then left feeder and vice versa left_feeder = programItem.GetPose()[0].GetPose().GetPosition().GetX() < 1f; if (left_feeder) { arm_rotation = Quaternion.Euler(-90f, -90f, 0f); } else { arm_rotation = Quaternion.Euler(90f, -90f, 0f); } gripper_pose = programItem.GetPose()[0].GetPose(); gripper_init_pose = new Vector3(gripper_pose.GetPosition().GetX(), -gripper_pose.GetPosition().GetY(), gripper_pose.GetPosition().GetZ()); moving_to_object = true; }
// 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 (!placedToPose) { runTime += Time.deltaTime; if (objectToPlace != null) { //grab the object and move it up if (moving_up) { objectToPlace.transform.localPosition = Vector3.Lerp(objectToPlace.transform.localPosition, upPosition, Time.deltaTime * 2f); if (ObjectInPosition(objectToPlace, upPosition)) { moving_up = false; moving_to_place = true; } } //move the object above the place position if (moving_to_place) { objectToPlace.transform.localPosition = Vector3.Lerp(objectToPlace.transform.localPosition, new Vector3(placePose.GetPosition().GetX(), -placePose.GetPosition().GetY(), objectToPlace.transform.localPosition.z), Time.deltaTime * 2f); objectToPlace.transform.localRotation = Quaternion.Lerp(objectToPlace.transform.localRotation, placeQuaternion, Time.deltaTime * 2f); if (ObjectInPosition(objectToPlace, new Vector3(placePose.GetPosition().GetX(), -placePose.GetPosition().GetY(), objectToPlace.transform.localPosition.z))) { moving_to_place = false; moving_down = true; } } //move the object down to place pose if (moving_down) { objectToPlace.transform.localPosition = Vector3.Lerp(objectToPlace.transform.localPosition, placePosition, Time.deltaTime * 2f); if (ObjectInPosition(objectToPlace, placePosition)) { moving_down = false; moving_arm_up = true; } } //move robot gripper up from the object if (moving_arm_up) { //release arm from attached state and set it to be the parent of world anchor if (pr2_arm.transform.parent != world_anchor.transform) { pr2_arm.transform.parent = world_anchor.transform; //open the gripper pr2_animator.SetBool("open_gripper", true); } //if gripper is opened then move the arm up if (pr2_animator.GetCurrentAnimatorStateInfo(0).IsName("opened")) { //prevent from opening again and again pr2_animator.SetBool("open_gripper", false); pr2_arm.transform.localPosition = Vector3.Lerp(pr2_arm.transform.localPosition, placePosition + new Vector3(0, 0, 0.3f), Time.deltaTime * 2f); if (ObjectInPosition(pr2_arm, placePosition + new Vector3(0, 0, 0.3f))) { moving_arm_up = false; //object is finally placed to position placedToPose = true; } } } } } else { run = false; } } } }