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 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 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 __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_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 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 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 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 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):
"""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 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 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 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 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 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 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 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 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 play_and_record(self,
motion_name,
groups=['right_arm'],
bag_name="no_bag_name_set"):
"""Play the specified motion and start recording poses.
Try to get the joints to record from the metadata of the play_motion gesture
or, optionally, specify the joints to track"""
# Check if motion exists in param server
PLAYMOTIONPATH = '/play_motion/motions/'
if not rospy.has_param(PLAYMOTIONPATH + motion_name):
rospy.logerr("Motion named: " + motion_name +
" does not exist in param server at " +
PLAYMOTIONPATH + motion_name)
return
else:
rospy.loginfo("Found motion " + motion_name +
" in param server at " + PLAYMOTIONPATH +
motion_name)
# Get it's info
motion_info = rospy.get_param(PLAYMOTIONPATH + motion_name)
# check if joints was specified, if not, get the joints to actually save
if len(groups) > 0:
joints_to_record = groups
else:
joints_to_record = motion_info['groups']
rospy.loginfo("Got groups: " + str(joints_to_record))
# play motion
rospy.loginfo("Playing motion!")
pm_goal = PlayMotionGoal(motion_name, False, 0)
self.play_motion_as.send_goal(pm_goal)
self.lfee.start_learn(motion_name, bag_name)
done_with_motion = False
while not done_with_motion:
state = self.play_motion_as.get_state()
#rospy.loginfo("State is: " + str(state) + " which is: " + goal_status_dict[state])
if state == GoalStatus.ABORTED or state == GoalStatus.SUCCEEDED:
done_with_motion = True
elif state != GoalStatus.PENDING and state != GoalStatus.ACTIVE:
rospy.logerr("We got state " + str(state) +
" unexpectedly, motion failed. Aborting.")
return None
rospy.sleep(0.1)
# data is written to rosbag in here
motion_data = self.lfee.stop_learn()
return motion_data
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 {}