def build(self):
self.init = states_cic.IdlePrimitive(
self.env,
kinematics=self.main_ctrl.kinematics,
params=self.parameters)
self.done = states.FailureState(self.env)
self.goinitpose = states_cic.InitResetPrimitive(
self.env,
kinematics=self.main_ctrl.kinematics,
params=self.parameters)
self.grasp = cic_grasping_class.ThreeFingerGrasp(
self.main_ctrl.kinematics, self.parameters, self.object)
self.approach = states_cic.ApproachObjectViapoints(
self.env,
self.main_ctrl.kinematics,
self.parameters,
self.object,
self.grasp,
self.parameters.approach_grasp_xy,
self.parameters.approach_grasp_h,
self.parameters.approach_duration,
self.parameters.approach_clip_pos,
stop_early=True,
assign_fingers=True)
self.move_cube_floor = states_cic.MoveCubeFloorPrimitiveLvl1(
self.env, self.main_ctrl.kinematics, self.parameters, self.object,
self.grasp, False)
self.reset_state = states_cic.ApproachObjectViapoints(
self.env,
self.main_ctrl.kinematics,
self.parameters,
self.object,
self.grasp,
self.parameters.reset_grasp_xy,
self.parameters.reset_grasp_h,
self.parameters.reset_duration,
self.parameters.reset_clip_pos,
stop_early=True,
assign_fingers=False)
#
self.init.connect(next_state=self.goinitpose, done_state=self.done)
# self.init.connect(next_state=self.init, done_state=self.done)
self.goinitpose.connect(next_state=self.approach, done_state=self.done)
self.approach.connect(next_state=self.move_cube_floor,
done_state=self.done)
self.move_cube_floor.connect(next_state=self.reset_state,
done_state=self.done)
self.reset_state.connect(next_state=self.init, done_state=self.done)
self.goto_init_pose = states.GoToInitPoseState(self.env)
# return initial state
return self.init
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 = 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)