def __init__(self, n_steps, step_length, step_height, step_time):

        assert n_steps >= 2, "Must specify at least two steps"

        self.n_steps = n_steps  # number of steps to take
        self.step_length = step_length  # how far forward each step goes
        self.step_height = step_height  # maximum height of the swing foot
        self.step_time = step_time  # how long each swing phase lasts

        self.n_phases = 2 * n_steps + 1  # how many different phases (double support, left support,
        # right support, etc. there are throughout the whole motion.
        self.total_time = self.step_time * self.n_phases

        # initial CoM and foot positions
        self.x_com_init = np.asarray([[0.0], [0.0], [1.04]])
        self.xd_com_init = np.asarray([[0.0], [0.0], [0.0]])

        self.fc_offset = -0.065  # The foot frame is this far from the foot's center

        self.x_right_init = np.asarray([[self.fc_offset], [-0.138], [0.1]])
        self.x_left_init = np.asarray([[self.fc_offset], [0.138], [0.1]])

        self.foot_w1 = 0.08  # width left of foot frame
        self.foot_w2 = 0.08  # width right of foot frame
        self.foot_l1 = 0.2  # length in front of foot
        self.foot_l2 = 0.07  # length behind foot

        # LIP parameters
        self.h = self.x_com_init[2]
        self.g = 9.81

        # Generate foot ground contact placements for both feet
        self.right_foot_placements = [self.x_right_init]
        self.left_foot_placements = [self.x_left_init]

        for step in range(self.n_steps):
            if (step == 0) or (step == self.n_steps - 1):
                # This is the first or last step, so shorten the stride
                l = self.step_length / 2
            else:
                l = self.step_length
            if step % 2 == 0:
                # Step with the right foot
                x_right = pycopy(self.right_foot_placements[-1])
                x_right[0, 0] += l
                self.right_foot_placements.append(x_right)
            else:
                # Step with the left foot
                x_left = pycopy(self.left_foot_placements[-1])
                x_left[0, 0] += l
                self.left_foot_placements.append(x_left)

        # Generate ZMP trajectory as function of time
        self._generate_zmp_trajectory()

        # Generate CoM trajectory as function of time using the LIP
        self._generate_com_trajectory()

        # Generate foot trajectories as functions of time.
        self._generate_foot_trajectories()
 def RightFootTrajectory(self, time):
     """
     Specify a desired position and velocity for the right foot
     """
     x_right = pycopy(self.x_right_init)
     xd_right = np.array([[0.0], [0.0], [0.0]])
     return (pycopy(x_right), pycopy(xd_right))
Пример #3
0
 def get_raw_array(self):
     namelength = self.read_element()[0]
     names = self.read_element()
     field_names = [names[i:i+namelength].tostring().strip('\x00')
                    for i in xrange(0,len(names),namelength)]
     tupdims = tuple(self.header['dims'][::-1])
     length = np.product(tupdims)
     if self.struct_as_record:
         if not len(field_names):
             # If there are no field names, there is no dtype
             # representation we can use, falling back to empty
             # object
             return np.empty(tupdims, dtype=object).T
         dtype = [(field_name, object) for field_name in field_names]
         result = np.empty(length, dtype=dtype)
         for i in range(length):
             for field_name in field_names:
                 result[i][field_name] = self.read_element()
     else: # Backward compatibility with previous format
         self.obj_template = mat_struct()
         self.obj_template._fieldnames = field_names
         result = np.empty(length, dtype=object)
         for i in range(length):
             item = pycopy(self.obj_template)
             for name in field_names:
                 item.__dict__[name] = self.read_element()
             result[i] = item
     return result.reshape(tupdims).T
 def LeftFootTrajectory(self, time):
     """
     Specify a desired position and velocity for the left foot
     """
     x_left = pycopy(self.x_left_init)
     xd_left = np.array([[0.0], [0.0], [0.0]])
     return (x_left, xd_left)
Пример #5
0
	def copy (self):
		"""
		Copy a channel using `copy.copy`
		@returns:
			The copied channel
		"""
		return pycopy(self)
Пример #6
0
	def expand (self, col = 0, pattern = "*"):
		"""
		expand the channel according to the files in <col>, other cols will keep the same
		`[(dir1/dir2, 1)].expand (0, "*")` will expand to
		`[(dir1/dir2/file1, 1), (dir1/dir2/file2, 1), ...]`
		length: 1 -> N
		width:  M -> M
		@params:
			`col`:     the index of the column used to expand
			`pattern`: use a pattern to filter the files/dirs, default: `*`
		@returns:
			The expanded channel
			Note, self is also changed
		"""
		from glob import glob
		import os
		folder = self[0][col]
		files  = glob (os.path.join(folder, pattern))
		
		tmp = list (self[0])
		for i, f in enumerate(files):
			n = pycopy(tmp)
			n [col] = f
			if i == 0:
				self[i] = tuple(n)
			else:
				self.append (tuple(n))
		return self
Пример #7
0
 def get_raw_array(self):
     namelength = self.read_element()[0]
     names = self.read_element()
     field_names = [
         names[i:i + namelength].tostring().strip('\x00')
         for i in xrange(0, len(names), namelength)
     ]
     tupdims = tuple(self.header['dims'][::-1])
     length = np.product(tupdims)
     if self.struct_as_record:
         if not len(field_names):
             # If there are no field names, there is no dtype
             # representation we can use, falling back to empty
             # object
             return np.empty(tupdims, dtype=object).T
         dtype = [(field_name, object) for field_name in field_names]
         result = np.empty(length, dtype=dtype)
         for i in range(length):
             for field_name in field_names:
                 result[i][field_name] = self.read_element()
     else:  # Backward compatibility with previous format
         self.obj_template = mat_struct()
         self.obj_template._fieldnames = field_names
         result = np.empty(length, dtype=object)
         for i in range(length):
             item = pycopy(self.obj_template)
             for name in field_names:
                 item.__dict__[name] = self.read_element()
             result[i] = item
     return result.reshape(tupdims).T
 def ComTrajectory(self, time):
     """
     Return a desired center of mass position and velocity for the given timestep
     """
     x_com = pycopy(self.x_com_init)
     xd_com = np.array([[0.0], [0.0], [0.0]])
     xdd_com = np.array([[0.0], [0.0], [0.0]])
     return (x_com, xd_com, xdd_com)
Пример #9
0
def goStraight(vehicle, nextNode, accumCost, route, Q, d):
    withoutDepotCost = float("inf")
    vehicleCopy = pycopy(vehicle)   
    distanceToNextNode = distCustomers(vehicleCopy.currPos, nextNode)

    #Updating Accum Cost to reaching the new node
    accumCost = accumCost + distanceToNextNode    

    #Advancing the vehicle to reach client
    vehicleCopy.currPos = nextNode        

    #Serve client
    withoutDepotCost = analyzeExpectedCostForAllDemand(accumCost, vehicleCopy, route, Q, d)
    return withoutDepotCost
Пример #10
0
def satisfyDemand(demand, vehicle, accumCost, route, Q, d):
    vehicleCopy = pycopy(vehicle)

    distanceToDepot = distCustomers(vehicleCopy.currPos, route[-1])
    
    if vehicleCopy.currQ < demand:
        #Failure, we go to the depot and back
        vehicleCopy.currQ = vehicleCopy.currQ - demand + Q
        accumCost = accumCost + 2 * distanceToDepot
    else:
        #update the Q of the vehicle
        vehicleCopy.currQ = vehicleCopy.currQ - demand
    
    return expLenCost(accumCost, vehicleCopy, route, Q, d)    
Пример #11
0
def goToDepot(vehicle, nextNode, accumCost, route, Q, d):
    withDepotCost = float("inf")

    vehicleCopy = pycopy(vehicle)
    
    distanceToDepot = distCustomers(vehicle.currPos, route[-1])
    distanceFromDepotToNext = distCustomers(nextNode, route[-1])
    #Don't go to the depot if Im there or coming from there, or going there next
    if not (vehicle.currQ == Q or nextNode.ID == 0):
        #If we go to the depot, the cost needs to include the trip
        accumCost = accumCost + distanceToDepot + distanceFromDepotToNext

        #Load up the truck again
        vehicleCopy.currQ = Q
        
        #Advancing the vehicle to the next node
        vehicleCopy.currPos = nextNode

        #Analyze demand with the vehicle in the next node
        withDepotCost = analyzeExpectedCostForAllDemand(accumCost, vehicleCopy, route, Q, d)

    return withDepotCost
Пример #12
0
def expLen(accumCost, vehicleGiven, routeGiven, Q, d):

    route = routeGiven[:]
    vehicle = pycopy(vehicleGiven)
    if len(route) == 0:
        return accumCost

    nextNode = route[0]

    #If the vehicle is already at the next node, just advance the route
    if vehicle.currPos.ID == nextNode.ID:
        #advance the algo
        route.pop(0)
        return expLen(accumCost, vehicle, route, Q, d)

    #If the vehicle is in the depot, load it up
    if vehicle.currPos.ID == 0:
        vehicle.currQ = Q

    withDepotCost = goToDepot(vehicle, nextNode, accumCost, route, Q, d)        
    withoutDepotCost = goStraight(vehicle, nextNode, accumCost, route, Q, d)
    return (withDepotCost, withoutDepotCost)
Пример #13
0
 def get_item(self):
     item = pycopy(self.obj_template)
     for element in item._fieldnames:
         item.__dict__[element]  = self.read_element()
     return item
Пример #14
0
 def get_item(self):
     item = pycopy(self.obj_template)
     for element in item._fieldnames:
         item.__dict__[element] = self.read_element()
     return item
Пример #15
0
    def emit(self, record):
        """
		Emit the record.
		"""
        from .jobmgr import Jobmgr
        try:
            pbar = record.pbar if hasattr(record, 'pbar') else None
            if pbar == 'next':
                if self.prevbar:
                    self.stream.write("\n")
                self._emit(record, "\n")
            elif pbar is None:
                # break pbars
                if not "\n" in record.msg:
                    self._emit(record, "\n")
                else:
                    msgs = record.msg.splitlines()
                    for i, m in enumerate(msgs):
                        rec = pycopy(record)
                        rec.msg = m
                        if i == len(msgs) - 1 and m.startswith('...... max='):
                            delattr(rec, 'jobidx')
                        self._emit(rec, "\n")
                self.prevbar = None
            elif pbar is True:
                # pbar, replace previous pbar
                self.prevbar = record
                self._emit(record, "\r")
            elif not self.prevbar:
                # not pbar and not prev pbar
                justlen = Jobmgr.PBAR_SIZE + 32
                if hasattr(record, 'proc'):
                    justlen += len(record.proc) + 2
                if hasattr(record, 'jobidx'):
                    justlen += len(str(record.joblen)) * 3
                justlen = max(justlen, Jobmgr.PBAR_SIZE + 32)
                if not "\n" in record.msg:
                    record.msg = record.msg.ljust(justlen)
                    self._emit(record, "\n")
                else:
                    msgs = record.msg.splitlines()
                    for i, m in enumerate(msgs):
                        rec = pycopy(record)
                        if i == len(msgs) - 1 and m.startswith('...... max='):
                            rec.msg = m.ljust(justlen)
                            delattr(rec, 'jobidx')
                        else:
                            rec.msg = m.ljust(justlen)
                        self._emit(rec, "\n")
            else:
                # not pbar but prev pbar
                justlen = Jobmgr.PBAR_SIZE + 32
                if hasattr(self.prevbar, 'proc'):
                    justlen += len(self.prevbar.proc) + 2
                if hasattr(self.prevbar, 'jobidx'):
                    justlen += len(str(self.prevbar.joblen)) * 3
                justlen = max(justlen, Jobmgr.PBAR_SIZE + 32)
                if not "\n" in record.msg:
                    record.msg = record.msg.ljust(justlen)
                    self._emit(record, "\n")
                else:
                    msgs = record.msg.splitlines()
                    for i, m in enumerate(msgs):
                        rec = pycopy(record)
                        if i == len(msgs) - 1 and m.startswith('...... max='):
                            rec.msg = m.ljust(justlen)
                            delattr(rec, 'jobidx')
                        else:
                            rec.msg = m.ljust(justlen)
                        self._emit(rec, "\n")
                self._emit(self.prevbar, "\r")
        except (KeyboardInterrupt, SystemExit, IOError,
                EOFError):  # pragma: no cover
            raise
        except Exception:  # pragma: no cover
            self.handleError(record)
    def _generate_foot_trajectories(self):
        """
        Generate a desired (x,y,z) trajectory for each foot as a piecewise polynomial.
        Once this method has been called, the foot positions at a given time t can be
        accessed like
            left_foot_position = self.left_foot_trajectory.value(t).
        """
        # Specify break points for piecewise linear interpolation
        break_times = np.asarray([[0.5 * i * self.step_time]
                                  for i in range(2 * self.n_phases + 1)])

        # Knot points are positions of the right and left feet at the break times
        lf_knots = np.zeros((3, len(break_times)))
        rf_knots = np.zeros((3, len(break_times)))

        lf_dot_knots = np.zeros((3, len(break_times)))
        rf_dot_knots = np.zeros((3, len(break_times)))

        lf_idx = 0
        rf_idx = 0
        for i in range(len(break_times)):
            t = break_times[i, 0]
            phase_time = (t / self.step_time) % 4
            if 1 < phase_time and phase_time < 2:
                # Right foot is in the middle of its swing phase
                rf_ref = pycopy(self.right_foot_placements[rf_idx])
                rf_ref[
                    0] += self.step_length / 2  # move the foot forward to the midpoint of the stride
                rf_ref[2] += self.step_height  # and up the designated hight

                # Slight forward velocity of the swing foot
                rf_dot_knots[0, i] = self.step_length / self.step_time

                # Next setpoint will be at the next foot placement
                rf_idx += 1

                # left foot remains at the same place
                lf_ref = self.left_foot_placements[lf_idx]

            elif 3 < phase_time and phase_time < 4:
                # Left foot is in the middle of its swing phase
                lf_ref = pycopy(self.left_foot_placements[lf_idx])
                lf_ref[
                    0] += self.step_length / 2  # move the foot forward to the midpoint of the stride
                lf_ref[2] += self.step_height  # and up the designated hight

                # Next setpoint will be at the next foot placement
                lf_idx += 1

                # Slight forward velocity of the swing foot
                lf_dot_knots[0, i] = self.step_length / self.step_time

                # right foot remains at the same place
                rf_ref = self.right_foot_placements[rf_idx]
            else:
                # We're in double support: both feet stay at the same position
                lf_ref = self.left_foot_placements[lf_idx]
                rf_ref = self.right_foot_placements[rf_idx]

            lf_knots[:, i] = lf_ref[:, 0]
            rf_knots[:, i] = rf_ref[:, 0]

        # Perform piecewise linear interpolation
        #self.left_foot_trajectory = PiecewisePolynomial.FirstOrderHold(break_times,lf_knots)
        #self.right_foot_trajectory = PiecewisePolynomial.FirstOrderHold(break_times,rf_knots)
        self.left_foot_trajectory = PiecewisePolynomial.Cubic(
            break_times, lf_knots, lf_dot_knots)
        self.right_foot_trajectory = PiecewisePolynomial.Cubic(
            break_times, rf_knots, rf_dot_knots)