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()