def _get_arrow_endpoints(self, rel_vec2: np.array, draw_arrow_at: np.array \
) -> tp.Tuple[np.array, np.array]:
left_unit_vec = angles.flip_y( \
angles.get_unit_vector_after_rotating( \
angles.flip_y(rel_vec2), angles.deg_to_rad(150)) )
right_unit_vec = angles.flip_y( \
angles.get_unit_vector_after_rotating( \
angles.flip_y(rel_vec2), angles.deg_to_rad(210)) )
left_endpoint = draw_arrow_at + left_unit_vec * self._arrowhead_length
right_endpoint = draw_arrow_at + right_unit_vec * self._arrowhead_length
return left_endpoint, right_endpoint
def crosswind(wind_speed, wind_angle):
"""
Resolve any wind / angle combination into crosswind.
Positive is from Shooter's Right to Left (Wind from 90 degree)
@param wind_speed The wind velocity, in mi/hr.
@param wind_angle The angle from which the wind is coming, in degrees.
0 degrees is from straight ahead
90 degrees is from right to left
180 degrees is from directly behind
270 or -90 degrees is from left to right.
@return the crosswind velocity component, in mi/hr.
"""
w_angle = angles.deg_to_rad(wind_angle)
return (math.sin(w_angle) * wind_speed)
def headwind(wind_speed, wind_angle):
"""
Resolve any wind / angle combination into headwind.
Headwind is positive at {@code wind_angle=0}
@param wind_speed The wind velocity, in mi/hr.
@param wind_angle The angle from which the wind is coming, in degrees.
0 degrees is from straight ahead
90 degrees is from right to left
180 degrees is from directly behind
270 or -90 degrees is from left to right.
@return the headwind velocity component, in mi/hr.
"""
w_angle = angles.deg_to_rad(wind_angle)
return (math.cos(w_angle) * wind_speed)
def solve(drag_function, drag_coefficient, vi, sight_height, shooting_angle, zero_angle, wind_speed, wind_angle):
t = 0
dt = 0
v = 0
vx = 0
vx1 = 0
vy = 0
vy1 = 0
dv = 0
dvx = 0
dvy = 0
x = 0
y = 0
hwind = windage.headwind(wind_speed, wind_angle)
cwind = windage.crosswind(wind_speed, wind_angle)
gy = constants.GRAVITY * \
math.cos(angles.deg_to_rad((shooting_angle + zero_angle)))
gx = constants.GRAVITY * \
math.sin(angles.deg_to_rad((shooting_angle + zero_angle)))
vx = vi * math.cos(angles.deg_to_rad(zero_angle))
vy = vi * math.sin(angles.deg_to_rad(zero_angle))
# y is in feet
y = -sight_height/12
n = 0
hold_overs = points()
while True:
vx1 = vx
vy1 = vy
v = math.pow(math.pow(vx, 2)+math.pow(vy, 2), 0.5)
dt = 0.5/v
# Compute acceleration using the drag function retardation
dv = drag.retard(drag_function, drag_coefficient, v+hwind)
dvx = -(vx/v)*dv
dvy = -(vy/v)*dv
# Compute velocity, including the resolved gravity vectors.
vx = vx + dt*dvx + dt*gx
vy = vy + dt*dvy + dt*gy
if x/3 >= n:
if x > 0:
range_yards = round(x/3)
print("range_yards {}".format(range_yards))
# if range_yards == 400:
moa_correction = -angles.rad_to_moa(math.atan(y / x))
print("moa_correction {}". format(moa_correction))
path_inches = y*12
print("path_inches {}". format(path_inches))
impact_in = utils.moaToInch(moa_correction, x)
seconds = t+dt
print("seconds {}". format(seconds))
hold_overs.add_point(
holdover(range_yards, moa_correction, impact_in, path_inches, seconds))
n = n + 1
# Compute position based on average velocity.
x = x + dt * (vx+vx1)/2
y = y + dt * (vy+vy1)/2
if (math.fabs(vy) > math.fabs(3*vx) or n >= constants.BALLISTICS_COMPUTATION_MAX_YARDS):
break
t = t + dt
return hold_overs
def get_angular_velocity(self):
"""
Velocity reported by API is incorrect so it needs to be corrected by a factor of 2
"""
vel = AngularPosition()
api_stat = self.GetAngularVelocity(byref(vel))
if (NO_ERROR_KINOVA == api_stat):
if ("6dof" == self.arm_dof):
ret = [
deg_to_rad(vel.Actuators.Actuator1 * 2),
deg_to_rad(vel.Actuators.Actuator2 * 2),
deg_to_rad(vel.Actuators.Actuator3 * 2),
deg_to_rad(vel.Actuators.Actuator4 * 2),
deg_to_rad(vel.Actuators.Actuator5 * 2),
deg_to_rad(vel.Actuators.Actuator6 * 2),
deg_to_rad(vel.Fingers.Finger1 * 2) * FINGER_FACTOR,
deg_to_rad(vel.Fingers.Finger2 * 2) * FINGER_FACTOR,
deg_to_rad(vel.Fingers.Finger3 * 2) * FINGER_FACTOR
]
elif ("7dof" == self.arm_dof):
ret = [
deg_to_rad(vel.Actuators.Actuator1 * 2),
deg_to_rad(vel.Actuators.Actuator2 * 2),
deg_to_rad(vel.Actuators.Actuator3 * 2),
deg_to_rad(vel.Actuators.Actuator4 * 2),
deg_to_rad(vel.Actuators.Actuator5 * 2),
deg_to_rad(vel.Actuators.Actuator6 * 2),
deg_to_rad(vel.Actuators.Actuator7 * 2),
deg_to_rad(vel.Fingers.Finger1 * 2) * FINGER_FACTOR,
deg_to_rad(vel.Fingers.Finger2 * 2) * FINGER_FACTOR,
deg_to_rad(vel.Fingers.Finger3 * 2) * FINGER_FACTOR
]
else:
rospy.loginfo("Kinova API failed: GetAngularVelocity (%d)",
api_stat)
ret = []
self.handle_comm_err(api_stat)
return ret
def get_angular_position(self):
pos = AngularPosition()
api_stat = self.GetAngularPosition(byref(pos))
if (NO_ERROR_KINOVA == api_stat):
if ("6dof" == self.arm_dof):
ret = [
deg_to_rad(pos.Actuators.Actuator1),
deg_to_rad(pos.Actuators.Actuator2 - 180.0),
deg_to_rad(pos.Actuators.Actuator3 - 180.0),
deg_to_rad(pos.Actuators.Actuator4),
deg_to_rad(pos.Actuators.Actuator5),
deg_to_rad(pos.Actuators.Actuator6),
deg_to_rad(pos.Fingers.Finger1) * FINGER_FACTOR,
deg_to_rad(pos.Fingers.Finger2) * FINGER_FACTOR,
deg_to_rad(pos.Fingers.Finger3) * FINGER_FACTOR
]
elif ("7dof" == self.arm_dof):
ret = [
deg_to_rad(pos.Actuators.Actuator1),
deg_to_rad(pos.Actuators.Actuator2 - 180.0),
deg_to_rad(pos.Actuators.Actuator3),
deg_to_rad(pos.Actuators.Actuator4 - 180.0),
deg_to_rad(pos.Actuators.Actuator5),
deg_to_rad(pos.Actuators.Actuator6 - 180.0),
deg_to_rad(pos.Actuators.Actuator7),
deg_to_rad(pos.Fingers.Finger1) * FINGER_FACTOR,
deg_to_rad(pos.Fingers.Finger2) * FINGER_FACTOR,
deg_to_rad(pos.Fingers.Finger3) * FINGER_FACTOR
]
else:
rospy.loginfo("Kinova API failed: GetAngularPosition (%d)",
api_stat)
ret = []
self.handle_comm_err(api_stat)
return ret
("SynchroType", c_int), ("Limitations", Limitation)]
class SensorInfo(Structure):
_fields_ = [("Voltage", c_float), ("Current", c_float),
("AccelerationX", c_float), ("AccelerationY", c_float),
("AccelerationZ", c_float), ("ActuatorTemp1", c_float),
("ActuatorTemp2", c_float), ("ActuatorTemp3", c_float),
("ActuatorTemp4", c_float), ("ActuatorTemp5", c_float),
("ActuatorTemp6", c_float), ("ActuatorTemp7", c_float),
("FingerTemp1", c_float), ("FingerTemp2", c_float),
("FingerTemp3", c_float)]
NO_ERROR_KINOVA = 1
LARGE_ACTUATOR_VELOCITY = deg_to_rad(
35.0) #maximum velocity of large actuator (joints 1-3) (deg/s)
SMALL_ACTUATOR_VELOCITY = deg_to_rad(
45.0) #maximum velocity of small actuator (joints 4-6) (deg/s)
ANGULAR_POSITION = 2
CARTESIAN_VELOCITY = 7
ANGULAR_VELOCITY = 8
FINGER_FACTOR = (0.986111027 / 121.5)
LARGE_ACTUATOR_VELOCITY = deg_to_rad(
35.0) #maximum velocity of large actuator (joints 1-3) (deg/s)
SMALL_ACTUATOR_VELOCITY = deg_to_rad(
45.0) #maximum velocity of small actuator (joints 4-6) (deg/s)
JOINT_6DOF_VEL_LIMITS = [
LARGE_ACTUATOR_VELOCITY, LARGE_ACTUATOR_VELOCITY, LARGE_ACTUATOR_VELOCITY,