def main():
rospy.init_node("pick_n_place_joint_trajectory")
# rospy.on_shutdown(shutdown)
# rospy.wait_for_message("/robot/sim/started", Empty) # uncomment this lines in real robot
sm = smach.StateMachine(outcomes=['Done'])
global dmp0_traj
global dmp1_traj
global dmp2_traj
global dmp3_traj
global dmp4_traj
dmp0_traj = dmp_r_joint_trajectory_client.Trajectory()
dmp1_traj = dmp_r_joint_trajectory_client.Trajectory()
dmp2_traj = dmp_r_joint_trajectory_client.Trajectory()
dmp3_traj = dmp_r_joint_trajectory_client.Trajectory()
dmp4_traj = dmp_r_joint_trajectory_client.Trajectory()
with sm:
smach.StateMachine.add(
'Go_to_start_position',
Go_to_start_position(),
transitions={'Succeed': 'Go_to_gripper_position'})
smach.StateMachine.add('Go_to_gripper_position',
Go_to_gripper_position(),
transitions={'Succeed': 'Gripper_close'})
smach.StateMachine.add('Gripper_close',
Gripper_close(),
transitions={'Succeed': 'Go_back'})
smach.StateMachine.add('Go_back',
Go_back(),
transitions={'Succeed': 'Gripper_open'})
smach.StateMachine.add('Gripper_open',
Gripper_open(),
transitions={'Succeed': 'Go_forward'})
smach.StateMachine.add(
'Go_forward',
Go_forward(),
transitions={'Succeed': 'Go_back_to_start_position'})
smach.StateMachine.add('Go_back_to_start_position',
Go_back_to_start_position(),
transitions={'Succeed': 'Done'})
sis = smach_ros.IntrospectionServer('MY_SERVER', sm, '/SM_ROOT')
sis.start()
outcome = sm.execute()
rospy.spin()
def execute(self, userdata):
global limb_interface
global mode_no_state_trainsition_report
if not mode_no_state_trainsition_report:
hmm_state_switch_client(self.state)
write_exec_hist(self, "Go_back_to_start_position", userdata, False)
current_angles = [
limb_interface.joint_angle(joint)
for joint in limb_interface.joint_names()
]
baxter_r_arm_dmp.main(
config.recorded_go_back_to_start_position_path,
config.generalized_go_back_to_start_position_path, current_angles)
dmp4_traj = dmp_r_joint_trajectory_client.Trajectory()
dmp4_traj.parse_file(config.generalized_go_back_to_start_position_path)
dmp4_traj.start()
dmp4_traj.wait()
return 'Succeed'
def execute(self, userdata):
global limb_interface
global mode_no_state_trainsition_report
if not mode_no_state_trainsition_report:
hmm_state_switch_client(self.state)
write_exec_hist(self, "Go_forward", userdata, True)
current_angles = [
limb_interface.joint_angle(joint)
for joint in limb_interface.joint_names()
]
baxter_r_arm_dmp.main(config.recorded_go_forward_path,
config.generalized_go_forward_path,
current_angles)
dmp3_traj = dmp_r_joint_trajectory_client.Trajectory()
dmp3_traj.parse_file(config.generalized_go_forward_path)
dmp3_traj.start()
if wait_for_motion_and_detect_anomaly(dmp3_traj):
return 'NeedRecovery'
return 'Succeed'