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