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