def move_to_start_pose(start_pose, limb_name):
if limb_name == "left":
name = [
'left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1',
'left_w2'
]
position = start_pose
if limb_name == "right":
name = [
'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0',
'right_w1', 'right_w2'
]
position = start_pose
limb = baxter_interface.Limb(limb_name)
d = dict(zip(name, position))
traj = srv_action_client.Trajectory(limb_name)
traj.clear(limb_name)
traj.stop()
traj.add_point([d[k] for k in limb.joint_names()], 4)
traj.start()
rospy.loginfo("stabalize baxter")
traj.wait(1)
rospy.sleep(5)
def main():
rospy.init_node("pick_n_place_joint_trajectory")
rospy.on_shutdown(shutdown)
rospy.wait_for_message("/robot/sim/started", Empty)
ipdb.set_trace()
sm = smach.StateMachine(outcomes=['Done'])
sm.userdata.sm_pick_object_pose = Pose()
sm.userdata.sm_place_object_pose = Pose()
sm.userdata.sm_hover_distance = 0.15
global traj
global limb_interface
global limb
limb = 'right'
traj = srv_action_client.Trajectory(limb)
limb_interface = baxter_interface.limb.Limb(limb)
with sm:
smach.StateMachine.add('Go_to_Start_Position',Go_to_Start_Position(),
transitions={'Succeed':'Setting_Start_and_End_Pose'})
smach.StateMachine.add('Setting_Start_and_End_Pose',Setting_Start_and_End_Pose(),
transitions={'Succeed':'Add_Box_Gazebo_Model'},
remapping={'pick_object_pose':'sm_pick_object_pose',
'place_object_pose':'sm_place_object_pose'})
smach.StateMachine.add('Add_Box_Gazebo_Model',Add_Box_Gazebo_Model(),
transitions={'Succeed':'Go_to_Pick_Position'},
remapping={'pick_object_pose':'sm_pick_object_pose'})
smach.StateMachine.add('Go_to_Pick_Position',Go_to_Pick_Position(),
transitions={'Succeed':'Go_to_Place_Position',
'IK_Fail':'Go_to_Start_Position',
'Time_Out':'Go_to_Start_Position'},
remapping={'pick_object_pose':'sm_pick_object_pose',
'hover_distance':'sm_hover_distance'})
smach.StateMachine.add('Go_to_Place_Position',Go_to_Place_Position(),
transitions={'Succeed':'Done',
'IK_Fail':'Go_to_Start_Position',
'Time_Out':'Go_to_Start_Position'},
remapping={'place_object_pose':'sm_place_object_pose',
'pick_object_pose':'sm_pick_object_pose',
'hover_distance':'sm_hover_distance'})
sis = smach_ros.IntrospectionServer('MY_SERVER', sm, '/SM_ROOT')
sis.start()
outcome = sm.execute()
rospy.spin()
def move_to_start_pose():
name = [
'head_nod', 'head_pan', 'left_e0', 'left_e1', 'left_s0', 'left_s1',
'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0',
'right_s1', 'right_w0', 'right_w1', 'right_w2', 'torso_t0'
]
position = [
0.0, -0.045252433242619704, 0.38809713933500967, 1.226034144717417,
-0.9690923627466101, -0.9460826509283289, -1.092577816171386,
1.554689528521867, 2.037509981508801, -0.9901845985800346,
1.5063691337034764, 0.3321068405771921, -1.1470341341413182,
0.4463884092746554, 1.3023496889147164, -0.03298058693953639,
-12.565987119160338
]
left_limb = baxter_interface.Limb('left')
right_limb = baxter_interface.Limb('right')
d = dict(zip(name, position))
left_traj = srv_action_client.Trajectory('left')
left_traj.clear('left')
left_traj.stop()
left_traj.add_point([d[k] for k in left_limb.joint_names()], 4)
left_traj.start()
right_traj = srv_action_client.Trajectory('right')
right_traj.clear('right')
right_traj.stop()
right_traj.add_point([d[k] for k in right_limb.joint_names()], 4)
right_traj.start()
left_traj.wait(5)
right_traj.wait(5)
rospy.loginfo('sleep 5 secs to let camera stablize.')
rospy.sleep(5)
def move_to_start_pose():
name = [
'head_nod', 'head_pan', 'left_e0', 'left_e1', 'left_s0', 'left_s1',
'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0',
'right_s1', 'right_w0', 'right_w1', 'right_w2', 'torso_t0'
]
position = [
0.0, -0.04486893804564835, -0.022626216621309852, 0.7493496148820246,
-1.4005244593393829, -0.5564515308054339, -0.2811019793800021,
0.5821457090025145, 0.3136990711225671, -0.16106798272796843,
1.7276458623559472, 0.11428156869746332, -0.1606844875309971,
-0.5579855115933192, -1.5650438988400934, 0.10814564554592167,
-12.565987119160338
]
left_limb = baxter_interface.Limb('left')
right_limb = baxter_interface.Limb('right')
d = dict(zip(name, position))
left_traj = srv_action_client.Trajectory('left')
left_traj.clear('left')
left_traj.stop()
left_traj.add_point([d[k] for k in left_limb.joint_names()], 4)
left_traj.start()
right_traj = srv_action_client.Trajectory('right')
right_traj.clear('right')
right_traj.stop()
right_traj.add_point([d[k] for k in right_limb.joint_names()], 4)
right_traj.start()
left_traj.wait(5)
right_traj.wait(5)
rospy.loginfo('sleep 5 secs to let camera stablize.')
rospy.sleep(5)
def calibrate_right_arm():
rospy.loginfo('calibrating right arm...')
limb = 'right'
traj = srv_action_client.Trajectory(limb)
limb_interface = baxter_interface.limb.Limb(limb)
traj.add_pose_point(hardcoded_data.hover_pick_object_pose, 4.0)
traj.start()
traj.wait(5)
rospy.sleep(5)
from std_srvs.srv import Trigger
trigger = rospy.ServiceProxy('/robotiq_wrench_calibration_service',
Trigger)
resp = trigger()
rospy.sleep(5)
rospy.loginfo('done...')
def main():
global mode_no_state_trainsition_report
global mode_no_anomaly_detection
global sm
rospy.init_node("pick_n_place_joint_trajectory")
rospy.on_shutdown(shutdown)
if not mode_no_anomaly_detection:
if mode_use_manual_anomaly_signal:
rospy.Subscriber("/manual_anomaly_signal", std_msgs.msg.String,
callback_manual_anomaly_signal)
else:
rospy.Subscriber("/anomaly_detection_signal", std_msgs.msg.Header,
callback_hmm)
sm = smach.StateMachine(outcomes=['TaskFailed', 'TaskSucceed'])
global traj
global limb_interface
global limb
limb = 'right'
traj = srv_action_client.Trajectory(limb)
limb_interface = baxter_interface.limb.Limb(limb)
rospy.loginfo('Building state machine...')
with sm:
smach.StateMachine.add('Go_to_Start_Position',
Go_to_Start_Position(),
transitions={
'NeedRecovery': 'Recovery',
'Succeed': 'Go_to_Pick_Hover_Position'
})
smach.StateMachine.add('Go_to_Pick_Hover_Position',
Go_to_Pick_Hover_Position(),
transitions={
'NeedRecovery': 'Recovery',
'Succeed': 'Go_to_Pick_Position'
})
smach.StateMachine.add('Go_to_Pick_Position',
Go_to_Pick_Position(),
transitions={
'NeedRecovery': 'Recovery',
'Succeed':
'Go_to_Pick_Hover_Position_Again',
})
smach.StateMachine.add('Go_to_Pick_Hover_Position_Again',
Go_to_Pick_Hover_Position_Again(),
transitions={
'NeedRecovery': 'Recovery',
'Succeed': 'Go_to_Place_Hover_Position'
})
smach.StateMachine.add('Go_to_Place_Hover_Position',
Go_to_Place_Hover_Position(),
transitions={
'NeedRecovery': 'Recovery',
'Succeed': 'Go_to_Place_Position'
})
smach.StateMachine.add('Go_to_Place_Position',
Go_to_Place_Position(),
transitions={
'NeedRecovery': 'Recovery',
'Succeed': 'TaskSucceed'
})
# build Recovery states automatically
recovery_outcomes = ['RecoveryFailed']
recovery_state_transitions = {'RecoveryFailed': 'TaskFailed'}
for added_state in sm._states:
recovery_outcomes.append('Reenter_' + added_state)
recovery_state_transitions['Reenter_' + added_state] = added_state
smach.StateMachine.add('Recovery',
Recovery(outcomes=recovery_outcomes),
transitions=recovery_state_transitions)
rospy.loginfo('Done...')
send_image(
os.path.join(os.path.dirname(os.path.realpath(__file__)), 'green.jpg'))
rospy.loginfo('Bring up smach introspection server...')
sis = smach_ros.IntrospectionServer('MY_SERVER', sm, '/SM_ROOT')
sis.start()
rospy.loginfo('Done...')
if not mode_no_state_trainsition_report:
hmm_state_switch_client(0)
calibrate_right_arm()
rospy.loginfo('Start state machine execution...')
outcome = sm.execute()
rospy.loginfo('Done...')
if not mode_no_state_trainsition_report:
hmm_state_switch_client(0)
rospy.spin()
def main():
rospy.init_node("pick_n_place_joint_trajectory")
rospy.on_shutdown(shutdown)
rospy.Subscriber("/hmm_online_result", Hmm_Log, callback_hmm)
#rospy.wait_for_message("/robot/sim/started", Empty)
#ipdb.set_trace()
sm = smach.StateMachine(outcomes=['Done'])
sm.userdata.sm_pick_object_pose = Pose()
sm.userdata.sm_place_object_pose = Pose()
sm.userdata.sm_hover_distance = 0.15
global traj
global limb_interface
global limb
limb = 'right'
traj = srv_action_client.Trajectory(limb)
limb_interface = baxter_interface.limb.Limb(limb)
with sm:
smach.StateMachine.add(
'Go_to_Start_Position',
Go_to_Start_Position(),
transitions={'Succeed': 'Setting_Start_and_End_Pose'})
smach.StateMachine.add(
'Setting_Start_and_End_Pose',
Setting_Start_and_End_Pose(),
transitions={'Succeed': 'Go_to_Pick_Hover_Position'},
remapping={
'pick_object_pose': 'sm_pick_object_pose',
'place_object_pose': 'sm_place_object_pose'
})
smach.StateMachine.add('Go_to_Pick_Hover_Position',
Go_to_Pick_Hover_Position(),
transitions={'Succeed': 'Go_to_Pick_Position'},
remapping={
'pick_object_pose': 'sm_pick_object_pose',
'hover_distance': 'sm_hover_distance'
})
smach.StateMachine.add(
'Recovery',
Recovery(),
transitions={'Succeed': 'Go_to_Pick_Hover_Position'},
remapping={
'pick_object_pose': 'sm_pick_object_pose',
'hover_distance': 'sm_hover_distance'
})
smach.StateMachine.add('Go_to_Pick_Position',
Go_to_Pick_Position(),
transitions={
'Succeed': 'Go_to_Place_Hover_Position',
'retry': 'Recovery'
},
remapping={
'pick_object_pose': 'sm_pick_object_pose',
'hover_distance': 'sm_hover_distance'
})
smach.StateMachine.add('Go_to_Place_Hover_Position',
Go_to_Place_Hover_Position(),
transitions={'Succeed': 'Go_to_Place_Position'},
remapping={
'place_object_pose': 'sm_place_object_pose',
'hover_distance': 'sm_hover_distance'
})
smach.StateMachine.add('Go_to_Place_Position',
Go_to_Place_Position(),
transitions={'Succeed': 'Done'},
remapping={
'place_object_pose': 'sm_place_object_pose',
'hover_distance': 'sm_hover_distance'
})
sis = smach_ros.IntrospectionServer('MY_SERVER', sm, '/SM_ROOT')
hmm_state_switch_client(0)
sis.start()
outcome = sm.execute()
hmm_state_switch_client(0)
rospy.spin()
