def get_cubic_root(self):
# We have the equation x^2 D^2 + (1-x)^4 * C / h_min^2
# where x = sqrt(mu).
# We substitute x, which is sqrt(mu), with x = y + 1.
# It gives y^3 + py = q
# where p = (D^2 h_min^2)/(2*C) and q = -p.
# We use the Vieta's substution to compute the root.
# There is only one real solution y (which is in [0, 1] ).
# http://mathworld.wolfram.com/VietasSubstitution.html
# eps in the numerator is to prevent momentum = 1 in case of zero gradient
p = (self._dist_to_opt + eps)**2 * (self._h_min + eps)**2 / 2 / (self._grad_var + eps)
w3 = (-math.sqrt(p**2 + 4.0 / 27.0 * p**3) - p) / 2.0
w = math.copysign(1.0, w3) * math.pow(math.fabs(w3), 1.0/3.0)
y = w - p / 3.0 / (w + eps)
x = y + 1
if DEBUG:
logging.debug("p %f, den %f", p, self._grad_var + eps)
logging.debug("w3 %f ", w3)
logging.debug("y %f, den %f", y, w + eps)
return x
python类copysign()的实例源码
def process_transactions(self, trade_event, current_orders):
for order, txn in self.transact(trade_event, current_orders):
if txn.type == zp.DATASOURCE_TYPE.COMMISSION:
order.commission = (order.commission or 0.0) + txn.cost
else:
if txn.amount == 0:
raise zipline.errors.TransactionWithNoAmount(txn=txn)
if math.copysign(1, txn.amount) != order.direction:
raise zipline.errors.TransactionWithWrongDirection(
txn=txn, order=order)
if abs(txn.amount) > abs(self.orders[txn.order_id].amount):
raise zipline.errors.TransactionVolumeExceedsOrder(
txn=txn, order=order)
order.filled += txn.amount
if txn.commission is not None:
order.commission = ((order.commission or 0.0) +
txn.commission)
# mark the date of the order to match the transaction
# that is filling it.
order.dt = txn.dt
yield txn, order
def scrollEvent(self, dx=0, dy=0):
"""
Generate scroll events from parametters and displacement
@param int dx delta movement from last call on x axis
@param int dy delta movement from last call on y axis
@return float absolute distance moved this tick
"""
# Compute mouse mouvement from interger part of d * scale
self._scr_dx += dx * self._scr_xscale
self._scr_dy += dy * self._scr_yscale
_syn = False
if int(self._scr_dx):
self.relEvent(rel=Rels.REL_HWHEEL, val=int(copysign(1, self._scr_dx)))
self._scr_dx -= int(self._scr_dx)
_syn = True
if int(self._scr_dy):
self.relEvent(rel=Rels.REL_WHEEL, val=int(copysign(1, self._scr_dy)))
self._scr_dy -= int(self._scr_dy)
_syn = True
if _syn:
self.synEvent()
def mode_ROUND(self, x, y, range):
"""
If input value bellow deadzone range, output value is zero
If input value is above deadzone range,
output value is 1 (or maximum allowed)
"""
if y == 0:
# Small optimalization for 1D input, for example trigger
if abs(x) > self.upper:
return copysign(range, x)
return (0 if abs(x) < self.lower else x), 0
distance = sqrt(x*x + y*y)
if distance < self.lower:
return 0, 0
if distance > self.upper:
angle = atan2(x, y)
return range * sin(angle), range * cos(angle)
return x, y
def mode_LINEAR(self, x, y, range):
"""
Input value is scaled, so entire output range is covered by
reduced input range of deadzone.
"""
if y == 0:
# Small optimalization for 1D input, for example trigger
return copysign(
clamp(
0,
((x - self.lower) / (self.upper - self.lower)) * range,
range),
x
), 0
distance = clamp(self.lower, sqrt(x*x + y*y), self.upper)
distance = (distance - self.lower) / (self.upper - self.lower) * range
angle = atan2(x, y)
return distance * sin(angle), distance * cos(angle)
def _subplot_scrolled(self, event):
keys = event['key']
if not keys:
# at least 1 step per event but a maximum of 5
step = max(1, abs(int(event['step'])))
step = min(step, 5)
step = copysign(step, event['step'])
slice_model = self._get_slice_view(event).model()
index = int(slice_model.index + step)
if 0 <= index < len(slice_model):
self._context.update_index_for_direction(slice_model.index_direction, index)
elif keys.state(ctrl=True) or keys.state(shift=True):
x = event['x']
y = event['y']
step = max(1, abs(int(event['step'])))
step = copysign(step / 20.0, event['step'])
slice_view = self._get_slice_view(event)
if slice_view.zoom(x, y, step):
self._context_changed()
def _from_float(cls, f):
if isinstance(f, int): # handle integer inputs
return cls(f)
if not isinstance(f, float):
raise TypeError("argument must be int or float.")
if _math.isinf(f) or _math.isnan(f):
return cls(repr(f))
if _math.copysign(1.0, f) == 1.0:
sign = 0
else:
sign = 1
n, d = abs(f).as_integer_ratio()
#k = d.bit_length() - 1
k = _bit_length(d) - 1
result = _dec_from_triple(sign, str(n*5**k), -k)
if cls is Decimal:
return result
else:
return cls(result)
def assertFloatIdentical(self, x, y):
"""Fail unless floats x and y are identical, in the sense that:
(1) both x and y are nans, or
(2) both x and y are infinities, with the same sign, or
(3) both x and y are zeros, with the same sign, or
(4) x and y are both finite and nonzero, and x == y
"""
msg = 'floats {!r} and {!r} are not identical'
if math.isnan(x) or math.isnan(y):
if math.isnan(x) and math.isnan(y):
return
elif x == y:
if x != 0.0:
return
# both zero; check that signs match
elif math.copysign(1.0, x) == math.copysign(1.0, y):
return
else:
msg += ': zeros have different signs'
self.fail(msg.format(x, y))
def assertFloatsAreIdentical(self, x, y):
"""assert that floats x and y are identical, in the sense that:
(1) both x and y are nans, or
(2) both x and y are infinities, with the same sign, or
(3) both x and y are zeros, with the same sign, or
(4) x and y are both finite and nonzero, and x == y
"""
msg = 'floats {!r} and {!r} are not identical'
if isnan(x) or isnan(y):
if isnan(x) and isnan(y):
return
elif x == y:
if x != 0.0:
return
# both zero; check that signs match
elif copysign(1.0, x) == copysign(1.0, y):
return
else:
msg += ': zeros have different signs'
self.fail(msg.format(x, y))
def __init__(self, lattice, m, n):
self.lattice = lattice
self.m = m
self.n = n
# Chiral vector
self.c = lattice.pos(m,n)
self.magC = mag(self.c)
# Translation vector
d = gcd(2*n+m,2*m+n)
self.t = lattice.pos((2*n+m)/d, -(2*m+n)/d);
self.magT = mag(self.t)
# Chiral rotation matrix (rotate a1 along x-axis)
self.theta = acos(norm(self.c)[0]*copysign(1, self.c[1]))
self.rotM = np.array([
[cos(self.theta), sin(self.theta)],
[-sin(self.theta), cos(self.theta)]]).T
# Calculate atoms and bonds in unit cell
self._boundsErr = mag(lattice.pos(0,0,0) - lattice.pos(0,0,1))
self.indices = self._calcIndices(m, n)
self.atoms = self._calcAtoms(self.indices)
self.bonds = self._calcBonds(self.indices)
move_to_home.py 文件源码
项目:master_robot_strage
作者: nwpu-basketball-robot
项目源码
文件源码
阅读 21
收藏 0
点赞 0
评论 0
def start_run(self, at_home_door, distance, angular):
if at_home_door == True:
self.brake()
self.robot_move.move_to(x=self.last_move_goal)
return True
if distance > (self.line_distance + self.distance_tolerance):
self.vel.linear.x = self.x_speed
self.vel.linear.y = math.copysign(self.y_speed, self.move_direction)
elif distance < (self.line_distance - self.distance_tolerance):
self.vel.linear.x = self.x_speed * -1
self.vel.linear.y = self.y_speed
else:
self.vel.linear.x = 0
self.vel.linear.y = self.y_speed
if math.fabs(angular) < self.angular_tolerance:
self.vel.angular.z = 0
elif angular > 0:
self.vel.angular.z = -1 * self.z_speed
else:
self.vel.angular.z = self.z_speed
self.cmd_move_pub.publish(self.vel)
return False
move_in_robot.py 文件源码
项目:master_robot_strage
作者: nwpu-basketball-robot
项目源码
文件源码
阅读 28
收藏 0
点赞 0
评论 0
def turn(self, goal_angular):
# ???????????????,??????????????
# ????,????
rospy.loginfo('[robot_move_pkg]->move_in_robot.turn will turn %s'%goal_angular)
current_angular = start_angular = self.robot_state.get_robot_current_w()#??????????
r = rospy.Rate(100)
delta_angular = current_angular - start_angular
move_velocity = g_msgs.Twist()
while not rospy.is_shutdown() :
if abs(delta_angular - abs(goal_angular)) < self.turn_move_stop_tolerance:
break
current_angular = self.robot_state.get_robot_current_w()
move_velocity.angular.z = math.copysign(self.w_speed, goal_angular)
delta_angular += abs(abs(current_angular) - abs(start_angular) )
start_angular = current_angular
self.cmd_move_pub.publish(move_velocity) #???????????
r.sleep()
self.brake()
def assertFloatsAreIdentical(self, x, y):
"""assert that floats x and y are identical, in the sense that:
(1) both x and y are nans, or
(2) both x and y are infinities, with the same sign, or
(3) both x and y are zeros, with the same sign, or
(4) x and y are both finite and nonzero, and x == y
"""
msg = 'floats {!r} and {!r} are not identical'
if isnan(x) or isnan(y):
if isnan(x) and isnan(y):
return
elif x == y:
if x != 0.0:
return
# both zero; check that signs match
elif copysign(1.0, x) == copysign(1.0, y):
return
else:
msg += ': zeros have different signs'
self.fail(msg.format(x, y))
def assertFloatsAreIdentical(self, x, y):
"""assert that floats x and y are identical, in the sense that:
(1) both x and y are nans, or
(2) both x and y are infinities, with the same sign, or
(3) both x and y are zeros, with the same sign, or
(4) x and y are both finite and nonzero, and x == y
"""
msg = 'floats {!r} and {!r} are not identical'
if isnan(x) or isnan(y):
if isnan(x) and isnan(y):
return
elif x == y:
if x != 0.0:
return
# both zero; check that signs match
elif copysign(1.0, x) == copysign(1.0, y):
return
else:
msg += ': zeros have different signs'
self.fail(msg.format(x, y))
def _drive_to_1(
self,
speed: int,
pos_x: float,
pos_y: float
) -> None:
diff_x = pos_x - self._pos_x
diff_y = pos_y - self._pos_y
if abs(diff_x) > abs(diff_y):
direct = math.degrees(math.atan(diff_y/diff_x))
else:
fract = diff_x / diff_y
sign = math.copysign(1, fract)
direct = sign * 90 - math.degrees(math.atan(fract))
if diff_x < 0:
direct += 180
self._rotate_to(speed, direct)
def process_transactions(self, trade_event, current_orders):
for order, txn in self.transact(trade_event, current_orders):
if txn.type == zp.DATASOURCE_TYPE.COMMISSION:
order.commission = (order.commission or 0.0) + txn.cost
else:
if txn.amount == 0:
raise zipline.errors.TransactionWithNoAmount(txn=txn)
if math.copysign(1, txn.amount) != order.direction:
raise zipline.errors.TransactionWithWrongDirection(
txn=txn, order=order)
if abs(txn.amount) > abs(self.orders[txn.order_id].amount):
raise zipline.errors.TransactionVolumeExceedsOrder(
txn=txn, order=order)
order.filled += txn.amount
if txn.commission is not None:
order.commission = ((order.commission or 0.0) +
txn.commission)
# mark the date of the order to match the transaction
# that is filling it.
order.dt = txn.dt
yield txn, order
def drain_asset(positions, asset, quantity):
"""
Generic method of reducing the quantity of assets across an entire set of positions
This is hard to do manually because positions can duplicates and arbitrary quantities
Traverse the entire position set reducing quantities to zero until it hits the target reduction
"""
remaining_quantity = quantity
# get a list of positions that are opposite to the quantity we are draining
positions = [_ for _ in positions if _.asset == asset and copysign(1,_.quantity) == copysign(1, quantity * -1)]
for position in positions:
if abs(remaining_quantity) <= abs(position.quantity):
# there are enough quantity in this position to complete it
position.quantity += remaining_quantity
remaining_quantity = 0
return remaining_quantity
if abs(remaining_quantity) > abs(position.quantity):
# we are going to have some left over
remaining_quantity += position.quantity
position.quantity = 0
return remaining_quantity
def estimate(self, quote:Quote, quantity:int = None):
# quantity is only used for direction
quantity = copysign(1, quantity)
if quote.bid is None or quote.ask is None or quote.bid == 0.0 or quote.ask == 0.0:
raise Exception(
'SlippageEstimator.estimate: Cannot estimate a price if the bid or ask are None or 0.0')
if quantity is None or quantity == 0:
raise Exception(
'SlippageEstimator.estimate: Must provide a signed quantity to buy or sell.')
spread = (quote.ask - quote.bid) / 2
midpoint = quote.bid + spread
if quantity > 0:
return midpoint - spread * self.slippage
else:
return midpoint + spread * self.slippage
def assertFloatIdentical(self, x, y):
"""Fail unless floats x and y are identical, in the sense that:
(1) both x and y are nans, or
(2) both x and y are infinities, with the same sign, or
(3) both x and y are zeros, with the same sign, or
(4) x and y are both finite and nonzero, and x == y
"""
msg = 'floats {!r} and {!r} are not identical'
if math.isnan(x) or math.isnan(y):
if math.isnan(x) and math.isnan(y):
return
elif x == y:
if x != 0.0:
return
# both zero; check that signs match
elif math.copysign(1.0, x) == math.copysign(1.0, y):
return
else:
msg += ': zeros have different signs'
self.fail(msg.format(x, y))
def _check_battery_state(_battery_acpi_path):
"""
@return LaptopChargeStatus
"""
o = slerp(_battery_acpi_path+'/state')
batt_info = yaml.load(o)
rv = LaptopChargeStatus()
state = batt_info.get('charging state', 'discharging')
rv.charge_state = state_to_val.get(state, 0)
rv.rate = _strip_A(batt_info.get('present rate', '-1 mA'))
if rv.charge_state == LaptopChargeStatus.DISCHARGING:
rv.rate = math.copysign(rv.rate, -1) # Need to set discharging rate to negative
rv.charge = _strip_Ah(batt_info.get('remaining capacity', '-1 mAh'))
rv.voltage = _strip_V(batt_info.get('present voltage', '-1 mV'))
rv.present = batt_info.get('present', False)
rv.header.stamp = rospy.get_rostime()
return rv
def _simulateTemps(self, delta=1):
timeDiff = self.lastTempAt - time.time()
self.lastTempAt = time.time()
for i in range(len(self.temp)):
if abs(self.temp[i] - self.targetTemp[i]) > delta:
oldVal = self.temp[i]
self.temp[i] += math.copysign(timeDiff * 10, self.targetTemp[i] - self.temp[i])
if math.copysign(1, self.targetTemp[i] - oldVal) != math.copysign(1, self.targetTemp[i] - self.temp[i]):
self.temp[i] = self.targetTemp[i]
if self.temp[i] < 0:
self.temp[i] = 0
if abs(self.bedTemp - self.bedTargetTemp) > delta:
oldVal = self.bedTemp
self.bedTemp += math.copysign(timeDiff * 10, self.bedTargetTemp - self.bedTemp)
if math.copysign(1, self.bedTargetTemp - oldVal) != math.copysign(1, self.bedTargetTemp - self.bedTemp):
self.bedTemp = self.bedTargetTemp
if self.bedTemp < 0:
self.bedTemp = 0
def _simulateTemps(self, delta=1):
timeDiff = self.lastTempAt - time.time()
self.lastTempAt = time.time()
for i in range(len(self.temp)):
if abs(self.temp[i] - self.targetTemp[i]) > delta:
oldVal = self.temp[i]
self.temp[i] += math.copysign(timeDiff * 10, self.targetTemp[i] - self.temp[i])
if math.copysign(1, self.targetTemp[i] - oldVal) != math.copysign(1, self.targetTemp[i] - self.temp[i]):
self.temp[i] = self.targetTemp[i]
if self.temp[i] < self.ambientTemp:
self.temp[i] = self.ambientTemp
if abs(self.bedTemp - self.bedTargetTemp) > delta:
oldVal = self.bedTemp
self.bedTemp += math.copysign(timeDiff * 10, self.bedTargetTemp - self.bedTemp)
if math.copysign(1, self.bedTargetTemp - oldVal) != math.copysign(1, self.bedTargetTemp - self.bedTemp):
self.bedTemp = self.bedTargetTemp
if self.bedTemp < self.ambientTemp:
self.bedTemp = self.ambientTemp
def setf(self, value):
"""store /value/ into a binary format"""
exponentbias = (2**self.components[1])/2 - 1
m,e = math.frexp(value)
# extract components from value
s = math.copysign(1.0, m)
m = abs(m)
# convert to integrals
sf = 1 if s < 0 else 0
exponent = e + exponentbias - 1
m = m*2.0 - 1.0 # remove the implicit bit
mantissa = int(m * (2**self.components[2]))
# store components
result = bitmap.zero
result = bitmap.push( result, bitmap.new(sf,self.components[0]) )
result = bitmap.push( result, bitmap.new(exponent,self.components[1]) )
result = bitmap.push( result, bitmap.new(mantissa,self.components[2]) )
return self.__setvalue__( result[0] )
def getf(self):
"""convert the stored floating-point number into a python native float"""
exponentbias = (2**self.components[1])/2 - 1
res = bitmap.new( self.__getvalue__(), sum(self.components) )
# extract components
res,sign = bitmap.shift(res, self.components[0])
res,exponent = bitmap.shift(res, self.components[1])
res,mantissa = bitmap.shift(res, self.components[2])
if exponent > 0 and exponent < (2**self.components[2]-1):
# convert to float
s = -1 if sign else +1
e = exponent - exponentbias
m = 1.0 + (float(mantissa) / 2**self.components[2])
# done
return math.ldexp( math.copysign(m,s), e)
# FIXME: this should return NaN or something
Log.warn('float_t.getf : {:s} : Invalid exponent value : {:d}'.format(self.instance(), exponent))
return 0.0
def assertFloatsAreIdentical(self, x, y):
"""assert that floats x and y are identical, in the sense that:
(1) both x and y are nans, or
(2) both x and y are infinities, with the same sign, or
(3) both x and y are zeros, with the same sign, or
(4) x and y are both finite and nonzero, and x == y
"""
msg = 'floats {!r} and {!r} are not identical'
if isnan(x) or isnan(y):
if isnan(x) and isnan(y):
return
elif x == y:
if x != 0.0:
return
# both zero; check that signs match
elif copysign(1.0, x) == copysign(1.0, y):
return
else:
msg += ': zeros have different signs'
self.fail(msg.format(x, y))
def rect(r, phi):
_rect_special = [
[inf+nanj, None, -inf, complex(-float("inf"), -0.0), None, inf+nanj, inf+nanj],
[nan+nanj, None, None, None, None, nan+nanj, nan+nanj],
[0, None, complex(-0.0, 0.0), complex(-0.0, -0.0), None, 0, 0],
[0, None, complex(0.0, -0.0), 0, None, 0, 0],
[nan+nanj, None, None, None, None, nan+nanj, nan+nanj],
[inf+nanj, None, complex(float("inf"), -0.0), inf, None, inf+nanj, inf+nanj],
[nan+nanj, nan+nanj, nan, nan, nan+nanj, nan+nanj, nan+nanj]
]
if not math.isfinite(r) or not math.isfinite(phi):
if math.isinf(phi) and not math.isnan(r) and r != 0:
raise ValueError
if math.isinf(r) and math.isfinite(phi) and phi != 0:
if r > 0:
return complex(math.copysign(inf, math.cos(phi)),
math.copysign(inf, math.sin(phi)))
return complex(-math.copysign(inf, math.cos(phi)),
-math.copysign(inf, math.sin(phi)))
return _rect_special[_special_type(r)][_special_type(phi)]
return complex(r*math.cos(phi), r*math.sin(phi))
def assertFloatsAreIdentical(self, x, y):
"""assert that floats x and y are identical, in the sense that:
(1) both x and y are nans, or
(2) both x and y are infinities, with the same sign, or
(3) both x and y are zeros, with the same sign, or
(4) x and y are both finite and nonzero, and x == y
"""
msg = 'floats {!r} and {!r} are not identical'
if isnan(x) or isnan(y):
if isnan(x) and isnan(y):
return
elif x == y:
if x != 0.0:
return
# both zero; check that signs match
elif copysign(1.0, x) == copysign(1.0, y):
return
else:
msg += ': zeros have different signs'
self.fail(msg.format(x, y))
def assertFloatIdentical(self, x, y):
"""Fail unless floats x and y are identical, in the sense that:
(1) both x and y are nans, or
(2) both x and y are infinities, with the same sign, or
(3) both x and y are zeros, with the same sign, or
(4) x and y are both finite and nonzero, and x == y
"""
msg = 'floats {!r} and {!r} are not identical'
if math.isnan(x) or math.isnan(y):
if math.isnan(x) and math.isnan(y):
return
elif x == y:
if x != 0.0:
return
# both zero; check that signs match
elif math.copysign(1.0, x) == math.copysign(1.0, y):
return
else:
msg += ': zeros have different signs'
self.fail(msg.format(x, y))
def assertFloatsAreIdentical(self, x, y):
"""assert that floats x and y are identical, in the sense that:
(1) both x and y are nans, or
(2) both x and y are infinities, with the same sign, or
(3) both x and y are zeros, with the same sign, or
(4) x and y are both finite and nonzero, and x == y
"""
msg = 'floats {!r} and {!r} are not identical'
if isnan(x) or isnan(y):
if isnan(x) and isnan(y):
return
elif x == y:
if x != 0.0:
return
# both zero; check that signs match
elif copysign(1.0, x) == copysign(1.0, y):
return
else:
msg += ': zeros have different signs'
self.fail(msg.format(x, y))
pp_02_b_car_statespace_astar_question.py 文件源码
项目:slam-tutorial-code
作者: tuongngoc
项目源码
文件源码
阅读 26
收藏 0
点赞 0
评论 0
def segment_points(start_pose, curvature, length, delta_length):
"""Return points of segment, at delta_length intervals."""
l = 0.0
delta_length = copysign(delta_length, length)
points = []
while abs(l) < abs(length):
points.append(CurveSegment.end_pose(start_pose, curvature, l)[0:2])
l += delta_length
return points
# --------------------------------------------------------------------------
# Exploration of car's kinematic state space.
# --------------------------------------------------------------------------
# Allowed movements. These are given as tuples: (curvature, length).
# The list contains forward and backward movements.