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
def __init__(self, env): super().__init__(env) self.goto_init_pose = states.GoToInitPoseState(self.env) self.planned_grasp = states.PlannedGraspState(self.env)