def build(self):
self.goto_init_pose = cpc_states.GoToInitState(self.env)
self.init_after_preorient = cpc_states.GoToInitState(self.env)
self.preorient_object = mp_states.AlignObjectSequenceState(self.env)
self.align_to_object = cpc_states.AlignState(self.env)
self.lower = cpc_states.LowerState(self.env)
self.grasp = cpc_states.IntoState(self.env)
self.move_to_goal = mp_states.MoveToGoalState(self.env)
self.failure = mp_states.FailureState(self.env)
# define transitions between states
self.goto_init_pose.connect(next_state=self.preorient_object,
failure_state=self.failure)
self.preorient_object.connect(next_state=self.init_after_preorient,
failure_state=self.failure)
self.init_after_preorient.connect(next_state=self.align_to_object,
failure_state=self.failure)
self.align_to_object.connect(next_state=self.lower,
failure_state=self.goto_init_pose)
self.lower.connect(next_state=self.grasp,
failure_state=self.goto_init_pose)
self.grasp.connect(next_state=self.move_to_goal,
failure_state=self.goto_init_pose)
self.move_to_goal.connect(
next_state=self.move_to_goal, failure_state=self.goto_init_pose)
return self.goto_init_pose
def build(self):
# THOSE 5 States are the skeleton
self.init = states_cic.IdlePrimitive(self.env)
self.align_obj = states.AlignObjectSequenceState(self.env)
self.goto_init_pos = states.GoToInitPoseState(self.env)
self.failure = states.FailureState(self.env)
self.do_nothing = states_cic.IdlePrimitiveLong(self.env)
self.build_grasp_and_approach()
# START CONNECTING THE STATE MACHINES
self.init.connect(next_state=self.align_obj, done_state=self.failure)
self.align_obj.connect(self.goto_init_pos, self.failure)
self.goto_init_pos.connect(self.grasp, self.failure)
self.grasp.connect(self.approach, self.failure)
self.approach.connect(self.do_nothing, self.do_nothing)
self.do_nothing.connect(next_state=self.do_nothing,
done_state=self.failure)
return self.init
def build(self):
self.goto_init_pose = cpc_states.GoToInitState(self.env)
self.init_after_preorient = cpc_states.GoToInitState(self.env)
self.preorient_object = mp_states.AlignObjectSequenceState(self.env)
self.planned_grasp = mp_states.PlannedGraspState(self.env)
self.move_to_goal = cpc_states.GoalWithOrientState(self.env)
self.failure = mp_states.FailureState(self.env)
# define transitions between states
self.goto_init_pose.connect(next_state=self.preorient_object,
failure_state=self.failure)
self.preorient_object.connect(next_state=self.init_after_preorient,
failure_state=self.goto_init_pose)
self.init_after_preorient.connect(next_state=self.planned_grasp,
failure_state=self.failure)
self.planned_grasp.connect(next_state=self.move_to_goal,
failure_state=self.goto_init_pose)
self.move_to_goal.connect(
next_state=self.move_to_goal, failure_state=self.goto_init_pose)
return self.goto_init_pose
def build(self):
self.goto_init_pose = states.GoToInitPoseState(self.env)
self.align_object = states.AlignObjectSequenceState(self.env)
self.planned_grasp = states.PlannedGraspState(self.env)
self.move_to_goal = states.MoveToGoalState(self.env)
self.wait = states.WaitState(self.env,
30 if self.env.simulation else 1000)
self.failure = states.FailureState(self.env)
# define transitions between states
self.goto_init_pose.connect(next_state=self.align_object,
failure_state=self.failure)
self.align_object.connect(next_state=self.planned_grasp,
failure_state=self.failure)
self.planned_grasp.connect(next_state=self.move_to_goal,
failure_state=self.align_object)
self.move_to_goal.connect(next_state=None, failure_state=self.wait)
self.wait.connect(next_state=self.goto_init_pose,
failure_state=self.failure)
# return initial state
return self.goto_init_pose
def build(self):
self.goto_init_pose = states.GoToInitPoseState(self.env)
self.align_object = states.AlignObjectSequenceState(self.env)
self.planned_grasp = states.PlannedGraspState(self.env)
self.grasp = cic_grasping_class.ThreeFingerGraspExternal(
self.main_ctrl.kinematics, self.parameters, self.object)
self.set_cic_grasp = states_cic.SetGraspFromOutsidePrimitive(
self.env, self.main_ctrl.kinematics, self.parameters, self.object,
self.grasp)
self.move_cube_lift = states_cic.MoveLiftCubePrimitiveLvl4(
self.env, self.main_ctrl.kinematics, self.parameters, self.object,
self.grasp, False)
self.wait = states.WaitState(self.env,
30 if self.env.simulation else 1000)
self.failure = states.FailureState(self.env)
# define transitions between states
self.goto_init_pose.connect(next_state=self.align_object,
failure_state=self.failure)
self.align_object.connect(next_state=self.planned_grasp,
failure_state=self.failure)
self.planned_grasp.connect(next_state=self.set_cic_grasp,
failure_state=self.align_object)
self.set_cic_grasp.connect(next_state=self.move_cube_lift,
done_state=self.failure)
self.move_cube_lift.connect(next_state=self.wait,
done_state=self.failure)
self.wait.connect(next_state=self.goto_init_pose,
failure_state=self.failure)
# return initial state
return self.goto_init_pose