else:
test_dist = round(fdist-fdistRem,2)
test_x = float(data[4])
test_y = float(data[5])
test_z = float(data[6])
dist = test_dist
height = test_height
pitch = test_pitch
TLH_RH = MakeTransform(matrix(rodrigues([0, 0, 0])),transpose(matrix([0.0, -dist, 0.0])))
# With the following input variables, trajectory generator should generate
# only 1 trajectory of length test_traj_length
traj = TrajectoryGenerator.get('jaemiPlanning', 'lift', test_traj_length,test_traj_length,1.0,0.05)
[nearestSphereIdx, minDist] = find_nearest_reachability_sphere(test_x, test_y, test_z, myRmaps[0].map)
# Note: tryTheseSpheres is a dictionary
[sortedKeys, tryTheseSpheres] = get_ranked_neighbors(nearestSphereIdx, myRmaps[0])
# tryTheseSpheres = [nearestSphereIdx]
# tryTheseSpheres.extend(myRmaps[0].map[nearestSphereIdx].neighbors)
stopTrying = False # This is for breaking out of trying the neighbors
iterationCount = 0
startTime = time.time()
for sIdx in sortedKeys:
rm2.load("jaemi_right_n8_m12_awesome")
print "Reachability map loaded for right arm."
initialWheelTransform = wheel.GetTransform()
myRmaps.append(rm2)
minLiftDist= 0.05
maxLiftDist = 0.5
delta1 = 0.05
delta2 = 0.05
myLogger = BensLogger(arg_note=str(datetime.now()),arg_name=myPathStr+'/humanoids2013_jaemiPlanning_lifting_samples')
myLogger.open('a')
myLogger.header(['label','pitch','height','traj_length','dist_left_right','left_start_sphere_ind','left_start_sphere_x','left_start_sphere_y','left_start_sphere_z','right_start_sphere_ind','right_start_sphere_x','right_start_sphere_y','right_start_sphere_z','timestamp','planning_time'])
resultCount = 0
for dist in TrajectoryGenerator.frange(0.1,0.5,0.1):
TLH_RH = MakeTransform(matrix(rodrigues([0, 0, 0])),transpose(matrix([0.0, -dist, 0.0])))
trajs = TrajectoryGenerator.get('jaemiPlanning', 'lift', minLiftDist,maxLiftDist,delta1,delta2)
#print trajs
resp = get_pairs(TLH_RH, env, robot, myRmaps)
if(resp != None):
pairs = resp[0]
rm = resp[1]
[relBaseConstraint] = resp[2]
for t in range(len(trajs[0])):
leftTraj = trajs[0][t]
rightTraj = trajs[1][t]
candidates = get_candidates(leftTraj, rightTraj, env, robot, myRmaps, probs[0], pairs, rm)
if(candidates != None):
for height in TrajectoryGenerator.frange(0.5,1.5,0.1):
for pitch in TrajectoryGenerator.frange(-pi/2,pi/2,pi/6):
test_dist = round(fdist - fdistRem, 2)
test_x = float(data[4])
test_y = float(data[5])
test_z = float(data[6])
dist = test_dist
height = test_height
pitch = test_pitch
totalTimeStart = time.time() # time in [seconds]
TLH_RH = MakeTransform(matrix(rodrigues([0, 0, 0])),
transpose(matrix([0.0, -dist, 0.0])))
trajs = TrajectoryGenerator.get('jaemiPlanning', 'lift',
test_traj_length, test_traj_length,
delta1, delta2)
#print trajs
resp = get_pairs(TLH_RH, env, robot, myRmaps)
if (resp != None):
pairs = resp[0]
rm = resp[1]
[relBaseConstraint] = resp[2]
for t in range(len(trajs[0])):
leftTraj = trajs[0][t]
rightTraj = trajs[1][t]
candidates = get_candidates(leftTraj, rightTraj, env,
robot, myRmaps, probs[0],
pairs, rm)
if (candidates != None):
# when the rotation around it's X axis is zero,
test_x = float(data[4])
test_y = float(data[5])
test_z = float(data[6])
dist = test_dist
height = test_height
pitch = test_pitch
TLH_RH = MakeTransform(matrix(rodrigues([0, 0, 0])),
transpose(matrix([0.0, -dist, 0.0])))
# With the following input variables, trajectory generator should generate
# only 1 trajectory of length test_traj_length
traj = TrajectoryGenerator.get('jaemiPlanning', 'rotcw',
test_traj_length, test_traj_length,
1.0, None, (dist * 0.5))
[nearestSphereIdx, minDist
] = find_nearest_reachability_sphere(test_x, test_y, test_z,
myRmaps[0].map)
# Note: tryTheseSpheres is a dictionary
[sortedKeys,
tryTheseSpheres] = get_ranked_neighbors(nearestSphereIdx,
myRmaps[0])
# tryTheseSpheres = [nearestSphereIdx]
# tryTheseSpheres.extend(myRmaps[0].map[nearestSphereIdx].neighbors)
stopTrying = False # This is for breaking out of trying the neighbors
delta1 = 0.05
delta2 = 0.05
myLogger = BensLogger(arg_note=str(datetime.now()),
arg_name=myPathStr +
'/humanoids2013_jaemiPlanning_pushing_samples')
myLogger.open('a')
myLogger.header([
'label', 'pitch', 'height', 'traj_length', 'dist_left_right',
'left_start_sphere_ind', 'left_start_sphere_x', 'left_start_sphere_y',
'left_start_sphere_z', 'right_start_sphere_ind',
'right_start_sphere_x', 'right_start_sphere_y', 'right_start_sphere_z',
'timestamp', 'planning_time'
])
resultCount = 0
for dist in TrajectoryGenerator.frange(0.1, 0.5, 0.1):
TLH_RH = MakeTransform(matrix(rodrigues([0, 0, 0])),
transpose(matrix([0.0, -dist, 0.0])))
trajs = TrajectoryGenerator.get('jaemiPlanning', 'push', minPushDist,
maxPushDist, delta1, delta2)
#print trajs
resp = get_pairs(TLH_RH, env, robot, myRmaps)
if (resp != None):
pairs = resp[0]
rm = resp[1]
[relBaseConstraint] = resp[2]
for t in range(len(trajs[0])):
leftTraj = trajs[0][t]
rightTraj = trajs[1][t]
candidates = get_candidates(leftTraj, rightTraj, env, robot,
myRmaps, probs[0], pairs, rm)
rm2.load("jaemi_right_n8_m12_awesome")
print "Reachability map loaded for right arm."
initialWheelTransform = wheel.GetTransform()
myRmaps.append(rm2)
minRotAngle = pi/8
maxRotAngle = pi/4
delta1 = pi/8
delta2 = None
myLogger = BensLogger(arg_note=str(datetime.now()),arg_name=myPathStr+'/humanoids2013_jaemiPlanning_turning_samples')
myLogger.open('a')
myLogger.header(['label','pitch','height','traj_length','dist_left_right','left_start_sphere_ind','left_start_sphere_x','left_start_sphere_y','left_start_sphere_z','right_start_sphere_ind','right_start_sphere_x','right_start_sphere_y','right_start_sphere_z','timestamp','planning_time'])
resultCount = 0
for dist in TrajectoryGenerator.frange(0.1,0.5,0.1):
TLH_RH = MakeTransform(matrix(rodrigues([0, 0, 0])),transpose(matrix([0.0, -dist, 0.0])))
trajs = TrajectoryGenerator.get('jaemiPlanning', 'rotcw', minRotAngle,maxRotAngle,delta1,delta2,(dist*0.5))
#print trajs
resp = get_pairs(TLH_RH, env, robot, myRmaps)
if(resp != None):
pairs = resp[0]
rm = resp[1]
[relBaseConstraint] = resp[2]
for t in range(len(trajs[0])):
leftTraj = trajs[0][t]
rightTraj = trajs[1][t]
candidates = get_candidates(leftTraj, rightTraj, env, robot, myRmaps, probs[0], pairs, rm)
if(candidates != None):
for height in TrajectoryGenerator.frange(0.5,1.5,0.1):
for pitch in TrajectoryGenerator.frange(-pi/2,pi/2,pi/6):
test_dist = round(0.05 + round(fdist-fdistRem,2))
else:
test_dist = round(fdist-fdistRem,2)
test_x = float(data[4])
test_y = float(data[5])
test_z = float(data[6])
dist = test_dist
height = test_height
pitch = test_pitch
totalTimeStart = time.time() # time in [seconds]
TLH_RH = MakeTransform(matrix(rodrigues([0, 0, 0])),transpose(matrix([0.0, -dist, 0.0])))
trajs = TrajectoryGenerator.get('jaemiPlanning', 'lift', test_traj_length, test_traj_length, delta1, delta2)
#print trajs
resp = get_pairs(TLH_RH, env, robot, myRmaps)
if(resp != None):
pairs = resp[0]
rm = resp[1]
[relBaseConstraint] = resp[2]
for t in range(len(trajs[0])):
leftTraj = trajs[0][t]
rightTraj = trajs[1][t]
candidates = get_candidates(leftTraj, rightTraj, env, robot, myRmaps, probs[0], pairs, rm)
if(candidates != None):
# when the rotation around it's X axis is zero,
# the wheel is facing to the ground.
# We add pi/2 to make it's zero the same with the hands
# Also when we say "SetTransform" it sets the transform
else:
test_dist = round(fdist-fdistRem,2)
test_x = float(data[4])
test_y = float(data[5])
test_z = float(data[6])
dist = test_dist
height = test_height
pitch = test_pitch
TLH_RH = MakeTransform(matrix(rodrigues([0, 0, 0])),transpose(matrix([0.0, -dist, 0.0])))
# With the following input variables, trajectory generator should generate
# only 1 trajectory of length test_traj_length
traj = TrajectoryGenerator.get('jaemiPlanning', 'rotcw', test_traj_length,test_traj_length,1.0,None,(dist*0.5))
[nearestSphereIdx, minDist] = find_nearest_reachability_sphere(test_x, test_y, test_z, myRmaps[0].map)
# Note: tryTheseSpheres is a dictionary
[sortedKeys, tryTheseSpheres] = get_ranked_neighbors(nearestSphereIdx, myRmaps[0])
# tryTheseSpheres = [nearestSphereIdx]
# tryTheseSpheres.extend(myRmaps[0].map[nearestSphereIdx].neighbors)
stopTrying = False # This is for breaking out of trying the neighbors
iterationCount = 0
startTime = time.time()
for sIdx in sortedKeys:
def getRefTraj():
refTraj, grip_states = TG.TrajectoryGenerator()
return refTraj, grip_states
delta1 = pi / 8
delta2 = None
myLogger = BensLogger(arg_note=str(datetime.now()),
arg_name=myPathStr +
'/humanoids2013_jaemiPlanning_turning_samples')
myLogger.open('a')
myLogger.header([
'label', 'pitch', 'height', 'traj_length', 'dist_left_right',
'left_start_sphere_ind', 'left_start_sphere_x', 'left_start_sphere_y',
'left_start_sphere_z', 'right_start_sphere_ind',
'right_start_sphere_x', 'right_start_sphere_y', 'right_start_sphere_z',
'timestamp', 'planning_time'
])
resultCount = 0
for dist in TrajectoryGenerator.frange(0.1, 0.5, 0.1):
TLH_RH = MakeTransform(matrix(rodrigues([0, 0, 0])),
transpose(matrix([0.0, -dist, 0.0])))
trajs = TrajectoryGenerator.get('jaemiPlanning', 'rotcw', minRotAngle,
maxRotAngle, delta1, delta2,
(dist * 0.5))
#print trajs
resp = get_pairs(TLH_RH, env, robot, myRmaps)
if (resp != None):
pairs = resp[0]
rm = resp[1]
[relBaseConstraint] = resp[2]
for t in range(len(trajs[0])):
leftTraj = trajs[0][t]
rightTraj = trajs[1][t]
candidates = get_candidates(leftTraj, rightTraj, env, robot,
