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)