def safe_place(self): rospy.loginfo("Moving arm to a safe pose") pmg = PlayMotionGoal() pmg.motion_name = 'home' pmg.skip_planning = False self.play_m_as.send_goal_and_wait(pmg) rospy.loginfo("Raise object done.")
def postgrasp_place(self): rospy.loginfo("Moving arm to post grasp pose") pmg = PlayMotionGoal() pmg.motion_name = 'pregrasp_weight' pmg.skip_planning = False self.play_m_as.send_goal_and_wait(pmg) rospy.loginfo("Raise object done.")
def look_around(self): rospy.loginfo("Start look around.") pmg = PlayMotionGoal() pmg.motion_name = 'head_look_around' pmg.skip_planning = False self.play_m_as.send_goal_and_wait(pmg) rospy.loginfo("Look around have been done.")
def start_aruco_home(self, req): rospy.loginfo("Go back to home") pmg = PlayMotionGoal() pmg.motion_name = 'home' pmg.skip_planning = False self.play_m_as.send_goal_and_wait(pmg) rospy.loginfo("Done.")
def _reset_arm_pose_with_play_motion(self): """ Reset the pose of robot arm in gazebo to home pose. We will reset the robot arm pose use play motion package. """ rospy.wait_for_service('/gazebo/unpause_physics') try: self.unpause_physics() except (rospy.ServiceException) as exc: print("/gazebo/unpause_physics service call failed:" + str(exc)) rospy.loginfo('wait for play motion action server') self.play_motion_client.wait_for_server() rospy.loginfo('connected to robot play motion server') # we can cancel the trajectory goal, if the controller cannot go to the # previous goal self.play_motion_client.cancel_goal() g = PlayMotionGoal() g.motion_name = 'unfold_arm' g.skip_planning = False rospy.loginfo('Sending goal with motion: ' + g.motion_name) self.play_motion_client.send_goal(g) rospy.loginfo('Waiting for results (reset)...') reset_ok = self.play_motion_client.wait_for_result(rospy.Duration(10)) state = self.play_motion_client.get_state() if reset_ok: rospy.loginfo('Reset successfully with state: ' + self._get_states_string(state)) else: rospy.loginfo('Reset failed with state: ' + self._get_states_string(state))
def execute(self, userdata): play_goal = PlayMotionGoal() play_goal.motion_name = userdata.manip_motion_to_play #play_goal.reach_time.secs = userdata.manip_time_to_play play_goal.skip_planning = userdata.skip_planning userdata.play_motion_sm_goal = play_goal return 'succeeded'
def execute(self, userdata): play_goal = PlayMotionGoal() play_goal.motion_name = userdata.manip_motion_to_play #play_goal.reach_time.secs = userdata.manip_time_to_play play_goal.skip_planning = False userdata.play_motion_sm_goal = play_goal return 'succeeded'
def on_play_2(self): change_to_controller('position') pm = SimpleActionClient('/play_motion', PlayMotionAction) pm.wait_for_server() pmg = PlayMotionGoal() pmg.motion_name = 'LBD_2X' pm.send_goal(pmg) pm.wait_for_result(rospy.Duration(0.1))
def prepare_robot_pandp(self): rospy.loginfo("Unfold arm safely") pmg = PlayMotionGoal() pmg.motion_name = 'pregrasp' pmg.skip_planning = False self.play_m_as.send_goal_and_wait(pmg) rospy.loginfo("Done.") rospy.loginfo("Robot prepared.")
def do_motion(x): #create the play motion goal and tell it which motion to perform print("doing motion") pmg = PlayMotionGoal() pmg.motion_name = x.data pmg.skip_planning = False #tell the client to send the goal pmc.send_goal_and_wait(pmg)
def execute(self, userdata): goal = PlayMotionGoal() goal.motion_name = 'home' goal.skip_planning = True global client client.send_goal(goal) client.wait_for_result(rospy.Duration(10.0)) rospy.loginfo("Arm tucked.") return 'outcome1'
def execute(self, userdata): goal = PlayMotionGoal() goal.motion_name = 'open_hand' goal.skip_planning = True global client client.send_goal(goal) client.wait_for_result(rospy.Duration(3.0)) rospy.loginfo("dropped") return 'outcome1'
def createPlayMotionGoal(motion_name, time): """Creates a PlayMotionGoal with the motion_name and time set. @arg motion_name string contains the motion name, i.e.: 'arms_t' @arg time int time in seconds to accomplish the goal @return PlayMotionGoal with the given parameters set""" pmg = PlayMotionGoal() pmg.motion_name = motion_name pmg.reach_time.secs = int(time) return pmg
def createPlayMotionGoal(motion_name): """Creates a PlayMotionGoal with the motion_name and time set. @arg motion_name string contains the motion name, i.e.: 'arms_t' @arg time int time in seconds to accomplish the goal @return PlayMotionGoal with the given parameters set""" pmg = PlayMotionGoal() pmg.motion_name = motion_name pmg.skip_planning = True return pmg
def prepare_robot(self): rospy.loginfo("Unfold arm safely") pmg = PlayMotionGoal() pmg.motion_name = 'pregrasp' pmg.skip_planning = False self.play_m_as.send_goal_and_wait(pmg) rospy.loginfo("Done unfolding arm.") self.lower_torso() self.lower_head() rospy.loginfo("Robot prepared.")
def play(self, motion_name, skip_planning=True): # send prescribed play motion goals self.play_motion_goal_sent = True play_goal = PlayMotionGoal() play_goal.motion_name = motion_name play_goal.skip_planning = skip_planning self.play_motion.send_goal_and_wait(play_goal) rospy.loginfo( 'Sent play_motion goal and waiting for robot to carry it out... ')
def playMotion(self, motion_name): # Wait for the play motion server to come up and send goal self.play_motion_client.wait_for_server(rospy.Duration(15.0)) # Create the play_motion goal and send it pose_goal = PlayMotionGoal() pose_goal.motion_name = motion_name pose_goal.skip_planning = True self.play_motion_client.send_goal(pose_goal) rospy.loginfo('Play motion goal sent') self.play_motion_client.wait_for_result()
def move_arm_home(self): rospy.loginfo("Fold arm safely") pmg = PlayMotionGoal() pmg.motion_name = 'home' pmg.skip_planning = False res = self.play_m_as.send_goal_and_wait(pmg) if res != GoalStatus.SUCCEEDED: return False self.scene.remove_world_object("table") rospy.loginfo("Done.") return True
def prepare_robot_nav(self): # Move torso to its maximum height self.move_torso("lift") # Raise arm rospy.loginfo("Moving arm to a safe pose") pmg = PlayMotionGoal() pmg.motion_name = 'pick_final_pose' pmg.skip_planning = False self.play_m_as.send_goal_and_wait(pmg) rospy.loginfo("Raise object done.")
def play_motion(motion_name): # Wait for the play motion server to come up and send goal play_motion_client = actionlib.SimpleActionClient('/play_motion', PlayMotionAction) play_motion_client.wait_for_server(rospy.Duration(15.0)) # Create the play_motion goal and send it pose_goal = PlayMotionGoal() pose_goal.motion_name = motion_name pose_goal.skip_planning = True play_motion_client.send_goal(pose_goal) play_motion_client.wait_for_result()
def action(name): client = actionlib.SimpleActionClient('/play_motion', PlayMotionAction) client.wait_for_server() goal = PlayMotionGoal() goal.motion_name = name goal.skip_planning = False goal.priority = 0 # Optional rospy.loginfo("Sending goal with motion: " + name) client.send_goal(goal) action_ok = client.wait_for_result(rospy.Duration(30.0))
def open_gripper(self): ''' Opens the gripper. The configuration is given somewhere in tiago_gazebo... ''' rospy.loginfo('Opening gripper') pmg = PlayMotionGoal() pmg.motion_name = 'open' pmg.skip_planning = False self.play_m_as.send_goal_and_wait(pmg) rospy.loginfo('Done opening gripper')
def unfold_arm(self): ''' Unfolds the robot arm. The configuration of the unfolded arm is given in config/pick_motions.yaml as 'pregrasp'. ''' rospy.loginfo("Unfold arm safely") pmg = PlayMotionGoal() pmg.motion_name = 'pregrasp' pmg.skip_planning = False self.play_m_as.send_goal_and_wait(pmg) rospy.loginfo("Done unfolding arm.")
def execute(self, userdata): goal = PlayMotionGoal() goal.motion_name = 'close_hand' goal.skip_planning = True global client client.send_goal(goal) client.wait_for_result(rospy.Duration(3.0)) rospy.loginfo("grabbed") pubfood = rospy.Publisher('food', Int8, queue_size=1, latch=True) pubfood.publish(1) rospy.sleep(0.1) return 'outcome1'
def play_motion(self, motion_name, block=True): # To check motions: # rosparam list | grep play_motion | grep joints # for example: # open_hand, close_hand, offer_hand, wave # shake_hands, thumb_up_hand, pointing_hand, pinch_hand # head_tour, do_weights, pick_from_floor g = PlayMotionGoal() g.motion_name = motion_name if block: self.ac.send_goal_and_wait(g) else: self.ac.send_goal(g)
def tiago_play_motion(motion, wait_duration=60.0): rospy.loginfo('Doing PlayMotion: ' + motion) pm = SimpleActionClient('/play_motion', PlayMotionAction) pm.wait_for_server() pmg = PlayMotionGoal() pmg.motion_name = motion pm.send_goal(pmg) if pm.wait_for_result(rospy.Duration.from_sec(wait_duration)): pm_result = pm.get_result() rospy.loginfo(pm_result) return pm_result else: rospy.logwarn("play motion timed out") return False
def play_motion_start_action(act_name): client = actionlib.SimpleActionClient('/play_motion', PlayMotionAction) goal = PlayMotionGoal() if (act_name == "head_down" or act_name == "head_tilt_down"): goal.motion_name = "head_tilt_down" elif (act_name == "head_normal" or act_name == "head_straight"): goal.motion_name = "head_normal" elif (act_name == "look_around" or act_name == "head_tour"): goal.motion_name = "head_pan_tilted" elif (act_name == "give_hand" or act_name == "offer_hand"): goal.motion_name = "reach_floor" elif (act_name == "home"): goal.motion_name = "home" else: # at the moment no more will be handled but there are plenty of more movements print("\t Wrong play_motion name...") return goal.skip_planning = False goal.priority = 0 client.wait_for_server() client.send_goal(goal) ''' action_ok = client.wait_for_result(rospy.Duration(2.5)) state = client.get_state() if action_ok: rospy.loginfo("Action finished succesfully with state: " + str(get_status_string(state))) else: rospy.logwarn("Action failed with state: " + str(get_status_string(state))) ''' return client
def move_arm_to_final(self): ''' Moves arm to the final pose after a successful pickup. The final pose of the arm is given in config/pick_motions.yaml as 'pick_final_pose'. Currently this flips the arm over causing the object to fall... ''' rospy.loginfo("Moving arm to a safe pose") pmg = PlayMotionGoal() pmg.motion_name = 'pick_final_pose' pmg.skip_planning = False self.play_m_as.send_goal_and_wait(pmg) rospy.loginfo("Raise object done.") rospy.loginfo("Done!")
def applyEndPosition(self): rospy.loginfo("Initializing dmp_execution test.") diction = [] for i in range(0, len(self.plan_resp.plan.points)): diction.append({ 'positions': self.plan_resp.plan.points[i].positions, 'time_from_start': self.plan_resp.plan.times[i] }) name = 'movement' d = { 'play_motion': { 'controllers': self.controllers, 'motions': { name: { 'joints': self.joint_names, 'points': diction } } } } # Create params rospy.set_param('/play_motion/motions/movement/points', diction) rospy.set_param('/play_motion/motions/movement/joints', self.joint_names) #rospy.loginfo("Parameters created") client = SimpleActionClient("play_motion", PlayMotionAction) client.wait_for_server() rospy.loginfo("...connected.") #rospy.wait_for_message("joint_states", JointState) #rospy.sleep(3.0) rospy.loginfo("Initial position...") goal = PlayMotionGoal() goal.motion_name = 'movement' goal.skip_planning = True #raw_input('Press Enter to execute action\n') client.send_goal(goal) client.wait_for_result(rospy.Duration(15.0)) rospy.loginfo("Movement reached.") rospy.loginfo("Action done")
def start_aruco_pick(self, req): rospy.loginfo("Unfold arm safely") pmg = PlayMotionGoal() pmg.motion_name = 'pregrasp_weight' pmg.skip_planning = False self.play_m_as.send_goal_and_wait(pmg) rospy.loginfo("Done.") # rospy.loginfo("Start look around.") # pmg = PlayMotionGoal() # pmg.motion_name = 'head_look_around' # pmg.skip_planning = False # self.play_m_as.send_goal_and_wait(pmg) # rospy.loginfo("Look around have been done.") self.pick_type.pick_aruco("pick") return {}
def place_aruco(self, string_operation): rospy.sleep(2.0) #self.prepare_robot() if string_operation == "place": # pick_g.object_pose.pose.position.z -= 0.1*(1.0/2.0) # Place the object back to its position rospy.loginfo("Gonna place near where it was") pick_g = PickUpPoseGoal() pick_g.object_pose.pose.position.x = 0.85 pick_g.object_pose.pose.position.y = 0.00 pick_g.object_pose.pose.position.z = 1.00 # pick_g.object_pose.pose.orientation = aruco_ps.pose.position # pick_g.object_pose.pose.position = aruco_ps.pose.position # pick_g.object_pose.pose.position.z -= 0.1*(1.0/2.0) rospy.loginfo("aruco pose in base_footprint:" + str(pick_g)) pick_g.object_pose.header.frame_id = 'base_footprint' pick_g.object_pose.pose.orientation.w = 1.0 pick_g.object_pose.pose.position.z += 0.05 self.place_as.send_goal_and_wait(pick_g) rospy.loginfo("Done!") # Raise arm rospy.loginfo("Moving arm to a safe pose") pmg = PlayMotionGoal() pmg.motion_name = 'pick_final_pose' pmg.skip_planning = False self.play_m_as.send_goal_and_wait(pmg) rospy.loginfo("Raise object done.") rospy.loginfo("Done!") self.place_finish.wait_for_service() finish_flag = send_flags() finish_flag.Request.flag = 1 self.place_finish.call(finish_flag) rospy.loginfo("call speak done.")
def reset(self): goal = PlayMotionGoal() goal.motion_name = 'home' goal.skip_planning = False goal.priority = 0 # Optional rospy.loginfo("Sending goal with motion: " + sys.argv[1]) self.client.send_goal(goal) rospy.loginfo("Waiting for result...") action_ok = self.client.wait_for_result(rospy.Duration(30.0)) state = self.client.get_state() if action_ok: rospy.loginfo("Action finished succesfully with state: " + str(get_status_string(state))) else: rospy.logwarn("Action failed with state: " + str(get_status_string(state)))
def play_motion(name, callback = None): goal = PlayMotionGoal() goal.skip_planning = False goal.motion_name = name goal.priority = 20 actions.send_goal(goal, callback)
from sensor_msgs.msg import JointState if __name__ == "__main__": rospy.init_node("grasp_demo") rospy.loginfo("Waiting for play_motion...") client = actionlib.SimpleActionClient("play_motion", PlayMotionAction) client.wait_for_server() rospy.loginfo("...connected.") rospy.wait_for_message("joint_states", JointState) rospy.sleep(3.0) rospy.loginfo("Grasping demo...") goal = PlayMotionGoal() goal.motion_name = 'look_at_object_demo' goal.skip_planning = True client.send_goal(goal) client.wait_for_result(rospy.Duration(5.0)) goal.motion_name = 'pregrasp_demo' goal.skip_planning = False client.send_goal(goal) client.wait_for_result(rospy.Duration(40.0)) goal.motion_name = 'grasp_demo' goal.skip_planning = True client.send_goal(goal) client.wait_for_result(rospy.Duration(10.0)) rospy.sleep(1.0)
if __name__ == '__main__': rospy.init_node('run_motion_python') if len(sys.argv) < 2: show_usage() exit(0) rospy.loginfo("Starting run_motion_python application...") wait_for_valid_time(10.0) client = SimpleActionClient('/play_motion', PlayMotionAction) rospy.loginfo("Waiting for Action Server...") client.wait_for_server() goal = PlayMotionGoal() goal.motion_name = sys.argv[1] goal.skip_planning = False goal.priority = 0 # Optional rospy.loginfo("Sending goal with motion: " + sys.argv[1]) client.send_goal(goal) rospy.loginfo("Waiting for result...") action_ok = client.wait_for_result(rospy.Duration(30.0)) state = client.get_state() if action_ok: rospy.loginfo("Action finished succesfully with state: " + str(get_status_string(state))) else: rospy.logwarn("Action failed with state: " + str(get_status_string(state)))