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,
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:
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) 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):
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): # when the rotation around it's X axis is zero, # the wheel is facing to the ground.
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): # when the rotation around it's X axis is zero, # the wheel is facing to the ground.
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
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:
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):
'/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(
initialWheelTransform = wheel.GetTransform() myRmaps.append(rm2) minPushDist= 0.05 maxPushDist = 0.5 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) 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): # when the rotation around it's X axis is zero, # the wheel is facing to the ground.