rospy.sleep(1)
####################### POSICION DE GRASP ###############################
pose.pose.position.x = 0.4508
pose.pose.position.y = -0.3605
pose.pose.position.z = 1.1423
pose.pose.orientation.w = 0.6812
pose.pose.orientation.x = -0.21744
pose.pose.orientation.y = 0.65471
pose.pose.orientation.z = 0.24387
pose.header.frame_id = robot.get_planning_frame()
pose.header.stamp = rospy.Time.now()
# move to a random target
group_right.set_random_target()
group_right.set_pose_target(pose, "r_eef")
result = group_right.go(None,1)
if result == 0:
print "ERROR Grasp position"
sys.exit()
####################### CLOSE RIGHT HAND ###############################
scene.remove_world_object("right_shoe")
rospy.sleep(1)
res = req(2,2) # 2 -> Right _hand 2-> Grasp
from moveit_commander import MoveGroupCommander
if name == '__main__':
group = MoveGroupCommander("arm")
# move to a random target
group.set_random_target()
group.go()
from moveit_commander import MoveGroupCommander
if name == '__main__':
group = MoveGroupCommander("arm")
# move to a random target
group.set_random_target()
group.go()
groups.append("r_arm")
groups.append("l_arm")
# move_group
move_group_right = MoveGroupCommander("r_arm")
move_group_left = MoveGroupCommander("l_arm")
#Get Current state
state = robot.get_current_variable_values()
####################### INIT POSITION RIGHT ARM ###############################
move_group_right.set_random_target()
result = move_group_right.go(None,0)
####################### INIT POSITION LEFT ARM ###############################
move_group_left.set_random_target()
result = move_group_left.go(None,0)
if result == 0:
print "ERROR Moving"
sys.exit()
