def adjust_to_shooting(): global last_proximity proximity = check_head_proximity() if proximity== True and last_proximity==False: PyPR2.moveBodyTo(0.0,0.0,(0.65)*PyPR2.getHeadPos()[0],1) #PyPR2.moveHeadTo(0.0,y) last_proximity = proximity
def adjust_to_shooting():
global last_proximity
proximity = check_head_proximity()
if proximity == True and last_proximity == False:
PyPR2.moveBodyTo(0.0, 0.0, (0.65) * PyPR2.getHeadPos()[0], 1)
#PyPR2.moveHeadTo(0.0,y)
last_proximity = proximity
def rotate_robot_to(desired_angle, time_to_reach = None, in_degrees = True):
global body_reached, body_failed
max_speed = math.pi/18.0 # (The maximum speed is 10 degrees per second)
if in_degrees:
gain = math.pi/180.0
else:
gain = 1.0
body_reached = False
body_failed = False
tau0 = body_angle(in_degrees = in_degrees)
d_theta = gain*(desired_angle - tau0)
if time_to_reach == None:
time_to_reach = abs(d_theta/max_speed)
PyPR2.moveBodyTo(0.0, 0.0, d_theta, time_to_reach + 2.0)
def take_robot_to(X, Y, time_to_reach = 5.0):
global body_reached, body_failed
body_reached = False
body_failed = False
rp = PyPR2.getRobotPose()
P0 = rp['position']
dX = X - P0[0]
dY = Y - P0[1]
tau = body_angle(in_degrees = False)
S = math.sin(tau)
C = math.cos(tau)
dx = C*dX + S*dY
dy = -S*dX + C*dY
PyPR2.moveBodyTo(dx, dy, 0.0, time_to_reach)
def move_robot_to(x, y , tau, time_to_reach = 5.0, in_degrees = True):
global body_reached, body_failed
if in_degrees:
gain = math.pi/180.0
else:
gain = 1.0
body_reached = False
body_failed = False
rp = PyPR2.getRobotPose()
p0 = rp['position']
dx = x - p0[0]
dy = y - p0[1]
'''
o = quat(rp['orientation']) # convert to cgkit quaternion
R = numpy.linalg.inv(o.toMat3())
'''
o = geo.Orientation_3D(rp['orientation'], representation = 'quaternion')
R = numpy.linalg.inv(o.matrix())
dp = numpy.dot(R.T, vecmat.as_vector([dx, dy, 0.0]))
tau0 = body_angle(in_degrees = in_degrees)
PyPR2.moveBodyTo( dp[0], dp[1], gain*(tau - tau0), time_to_reach)
def timerActions( id ):
global msgTryTimer,busy_moving,track_d,last_action_counter,d,start_time,x,y,elapsed_time,focus_obj,a
if msgTryTimer == id :
#PyPR2.removeTimer( msgTryTimer )
#msgTryTimer += 1
#while True:
#time.sleep(1)
if abs(a-focus_obj['est_pos'][0]) < 0.04:
a = focus_obj['est_pos'][0]
PyPR2.moveBodyTo(0.1,0.0,0.0,1)
else:
a = focus_obj['est_pos'][0]
d = math.sqrt(math.pow(x,2)+math.pow(y,2))
adjust_to_shooting()
'''
with open(csvfile, "w") as output:
writer = csv.writer(output, lineterminator='\n')
writer.writerows(track_d)
'''
#PyPR2.moveTorsoBy(0.03,5)
#if abs(previous_pos - focus_obj['est_pos'][0])< 0.1:
# PyPR2.moveHeadTo(0.2,1.0)
#adjust_to_shooting()
#if busymoving>0: busymoving-=1
if d>=3.5:
if last_action_counter <4 and last_action_counter!=0:
#obj.larm_reference = True
#obj.arm_down()
PyPR2.moveArmWithJointPos(**right_high_five)
PyPR.moveArmWithJointPos(**shooting_back_down)
last_action_counter=4
#busymoving=3
elif last_action_counter == 4:
PyPR2.moveTorsoBy(0.1,2)
PyPR2.say("Move Back")
#busymoving=5
else:
PyPR2.moveArmWithJointPos(**left_shooting)
last_action_counter=4
#busymoving=5
elif d<=3.5 and d>3:
if last_action_counter <3 and last_action_counter!=0:
PyPR2.moveArmWithJointPos(**shooting_down)
#obj.larm_reference = False
#obj.arm_right()
#PyPR2.moveBodyTo(0.01,0.0,0.0,1)
last_action_counter=3
elif last_action_counter ==3:
PyPR2.moveBodyTo(0.07,0.0,0.0,0.51)
#busymoving=10
elif last_action_counter ==0:
PyPR2.moveArmWithJointPos(**left_shooting)
PyPR2.moveArWithJointPos(**alt_right_shooting)
#busymoving=10
PyPR2.say("Move Back")
else:
PyPR2.moveArmWithJointPos(**alt_right_shooting)
PyPR2.moveArmWithJointPos(**left_shooting)
last_action_counter=3
elif d<=3 and d >2:
if last_action_counter <2 and last_action_counter!=0:
#obj.larm_reference = False
#obj.arm_right()
PyPR2.moveArmWithJointPos(**left_relax1)
last_action_counter=2
#elif last_action_counter==0:
else:
PyPR2.moveArmWithJointPos(**alt_right_shooting)
PyPR2.moveArmWithJointPos(**left_shooting)
PyPR2.closeGripper(2)
#PyPR2.closeGripper(2)
last_action_counter =2
#previous_pos = focus_obj['est_pos'][0]
#movement_tracker.append(str(CONDITION_TAG)+":"+str(focus_obj['est_pos']))
elif d <2 and d>1 :
if last_action_counter ==1:
PyPR2.say("Move")
last_action_counter=1
#busymoving=10
elif last_action_counter >=2:
PyPR2.moveArmWithJointPos(**left_shooting)
PyPR2.moveArmWithJointPos(**alt_right_shooting)
else :
PyPR2.moveArmWithJointPos(**alt_right_shooting)
PyPr2.moveArmWithJointPos(**left_shooting)
def timerActions(id):
global msgTryTimer, busy_moving, track_d, last_action_counter, d, start_time, x, y, elapsed_time, focus_obj, a
if msgTryTimer == id:
#PyPR2.removeTimer( msgTryTimer )
#msgTryTimer += 1
#while True:
#time.sleep(1)
if abs(a - focus_obj['est_pos'][0]) < 0.04:
a = focus_obj['est_pos'][0]
PyPR2.moveBodyTo(0.1, 0.0, 0.0, 1)
else:
a = focus_obj['est_pos'][0]
d = math.sqrt(math.pow(x, 2) + math.pow(y, 2))
adjust_to_shooting()
'''
with open(csvfile, "w") as output:
writer = csv.writer(output, lineterminator='\n')
writer.writerows(track_d)
'''
#PyPR2.moveTorsoBy(0.03,5)
#if abs(previous_pos - focus_obj['est_pos'][0])< 0.1:
# PyPR2.moveHeadTo(0.2,1.0)
#adjust_to_shooting()
#if busymoving>0: busymoving-=1
if d >= 3.5:
if last_action_counter < 4 and last_action_counter != 0:
#obj.larm_reference = True
#obj.arm_down()
PyPR2.moveArmWithJointPos(**right_high_five)
PyPR.moveArmWithJointPos(**shooting_back_down)
last_action_counter = 4
#busymoving=3
elif last_action_counter == 4:
PyPR2.moveTorsoBy(0.1, 2)
PyPR2.say("Move Back")
#busymoving=5
else:
PyPR2.moveArmWithJointPos(**left_shooting)
last_action_counter = 4
#busymoving=5
elif d <= 3.5 and d > 3:
if last_action_counter < 3 and last_action_counter != 0:
PyPR2.moveArmWithJointPos(**shooting_down)
#obj.larm_reference = False
#obj.arm_right()
#PyPR2.moveBodyTo(0.01,0.0,0.0,1)
last_action_counter = 3
elif last_action_counter == 3:
PyPR2.moveBodyTo(0.07, 0.0, 0.0, 0.51)
#busymoving=10
elif last_action_counter == 0:
PyPR2.moveArmWithJointPos(**left_shooting)
PyPR2.moveArWithJointPos(**alt_right_shooting)
#busymoving=10
PyPR2.say("Move Back")
else:
PyPR2.moveArmWithJointPos(**alt_right_shooting)
PyPR2.moveArmWithJointPos(**left_shooting)
last_action_counter = 3
elif d <= 3 and d > 2:
if last_action_counter < 2 and last_action_counter != 0:
#obj.larm_reference = False
#obj.arm_right()
PyPR2.moveArmWithJointPos(**left_relax1)
last_action_counter = 2
#elif last_action_counter==0:
else:
PyPR2.moveArmWithJointPos(**alt_right_shooting)
PyPR2.moveArmWithJointPos(**left_shooting)
PyPR2.closeGripper(2)
#PyPR2.closeGripper(2)
last_action_counter = 2
#previous_pos = focus_obj['est_pos'][0]
#movement_tracker.append(str(CONDITION_TAG)+":"+str(focus_obj['est_pos']))
elif d < 2 and d > 1:
if last_action_counter == 1:
PyPR2.say("Move")
last_action_counter = 1
#busymoving=10
elif last_action_counter >= 2:
PyPR2.moveArmWithJointPos(**left_shooting)
PyPR2.moveArmWithJointPos(**alt_right_shooting)
else:
PyPR2.moveArmWithJointPos(**alt_right_shooting)
PyPr2.moveArmWithJointPos(**left_shooting)