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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 tiago_head_default(wait=False):
# move head back to default
play_motion_client = actionlib.SimpleActionClient('/play_motion',
PlayMotionAction)
play_motion_client.wait_for_server()
pm_goal = PlayMotionGoal('back_to_default', True, 0)
test_goal = PlayMotionGoal()
print test_goal.priority
play_motion_client.send_goal(pm_goal)
rospy.loginfo('play motion: back to default')
if wait:
play_motion_client.wait_for_result()
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 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(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 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 __init__(self):
# initialise state variables
self._reset()
# manipulation services
s = rospy.Service('/gaan/manipulate', Manipulate,
self.manipulateHandler)
# manipulation clients
self.play_motion_client = SimpleActionClient('/play_motion',
PlayMotionAction)
rospy.loginfo("Waiting for Action Server...")
self.play_motion_client.wait_for_server()
self.playmotion_goal = PlayMotionGoal()
self.playmotion_goal.skip_planning = False
self.playmotion_goal.priority = 0 # Optional
self.pick_as = SimpleActionClient('/pickup_pose', PickUpPoseAction)
self.place_as = SimpleActionClient('/place_pose', PickUpPoseAction)
self.pickup_goal = PickUpPoseGoal()
self.pickup_goal.object_pose.header.frame_id = "base_footprint"
# end initialisation
rospy.loginfo("Ready to manipulate!")
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.")
import actionlib
from play_motion_msgs.msg import PlayMotionAction, PlayMotionGoal
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))
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:
def play_motion(name, callback = None): goal = PlayMotionGoal() goal.skip_planning = False goal.motion_name = name goal.priority = 20 actions.send_goal(goal, callback)
