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