Exemple #1
0
                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:
Exemple #3
0
                          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. 
Exemple #6
0
            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:
Exemple #9
0
                          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):
Exemple #10
0
                          '/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.