def ik(self, x, y, z):
"""
Calculates the inverse kinematics from a given foot coordinate (x,y,z)[mm]
and returns the joint angles[degrees]
Reference (there are typos)
https://tote.readthedocs.io/en/latest/ik.html
"""
try:
Lc = self.coxaLength
Lf = self.femurLength
Lt = self.tibiaLength
a = atan2(y, x) # <---
# a = atan2(x, y)
f = sqrt(x**2 + y**2) - Lc
b1 = atan2(z, f) # <---
# b1 = atan2(f, z)
d = sqrt(f**2 + z**2) # <---
b2 = acos((Lf**2 + d**2 - Lt**2) / (2.0 * Lf * d))
b = b1 + b2
g = acos((Lf**2 + Lt**2 - d**2) / (2.0 * Lf * Lt))
#### FIXES ###################################
g -= pi # fix to align fk and ik frames
##############################################
# print('ik angles: {:.2f} {:.2f} {:.2f}'.format(r2d(a), r2d(b), r2d(g)))
# return a, b, g # coxaAngle, femurAngle, tibiaAngle
return r2d(a), r2d(b), r2d(g) # coxaAngle, femurAngle, tibiaAngle
except Exception as e:
print('ik error:', e)
raise e
def geocoords(azrange,rlat,rlon,zoomlevel):
R=6371.0
az=d2r(azrange[0]) #Azimuth in radians
r=azrange[1]
rlat=d2r(rlat) #To radians
lat=asin(sin(rlat)*cos(r/R)+cos(rlat)*sin(r/R)*cos(az))
lon=rlon+r2d(atan2(sin(az)*sin(r/R)*cos(rlat),cos(r/R)-sin(rlat)*sin(lat)))
return round(r2d(lat),5),round(lon,5)
def ik(self, x, y, z):
"""
Calculates the inverse kinematics from a given foot coordinate (x,y,z)[mm]
and returns the joint angles[degrees]
Reference (there are typos)
https://tote.readthedocs.io/en/latest/ik.html
If the foot (x,y,z) is in a position that does not produce a result (some
numeric error or invalid foot location), the this returns None.
"""
# try:
Lc = self.coxaLength
Lf = self.femurLength
Lt = self.tibiaLength
if sqrt(x**2 + y**2) < Lc:
print('too short')
return None
# elif z > 0.0:
# return None
a = atan2(y, x)
f = sqrt(x**2 + y**2) - Lc
# b1 = atan2(z, f) # takes into account quadrent, z is neg
# you have different conditions depending if z is pos or neg
if z < 0.0:
b1 = atan2(f, fabs(z))
else:
b1 = atan2(z, f)
d = sqrt(f**2 + z**2) # <---
# print('ik pos: {} {} {}'.format(x,y,z))
# print('d: {:.3f} f: {:.3f}'.format(d,f))
# print('Lc Lf Lt: {} {} {}'.format(Lc,Lf,Lt))
# print('num: {:.3f}'.format(Lf**2 + d**2 - Lt**2))
# print('den: {:.3f}'.format(2.0 * Lf * d))
# print('acos: {:.2f}'.format((Lf**2 + d**2 - Lt**2) / (2.0 * Lf * d)))
guts = ((Lf**2 + d**2 - Lt**2) / (2.0 * Lf * d))
if 1.0 < guts or guts < -1.0:
print('acos crap!: {:.3f} {:.3f} {:.3f} guts: {:.3f}'.format(x, y, z, guts))
return None
b2 = acos((Lf**2 + d**2 - Lt**2) / (2.0 * Lf * d)) # issues?
b = b1 + b2
g = acos((Lf**2 + Lt**2 - d**2) / (2.0 * Lf * Lt))
# FIXES ###################################
g -= pi # fix to align fk and ik frames
if z < 0.0:
b -= pi/2 #
##############################################
# print('b1 b2: {:.2f} {:.2f}'.format(r2d(b1), r2d(b2)))
# print('ik angles: {:.2f} {:.2f} {:.2f}'.format(r2d(a), r2d(b), r2d(g)))
return r2d(a), r2d(b), r2d(g) # coxaAngle, femurAngle, tibiaAngle
def from_euler(r, p, y, rads=True, axes=321):
"""
Creates a quaternion given a set of euler angles.
Args:
r: roll
p: pitch
y: yaw
rads: are the angles in radians or degrees? default is rads
axes: order of axis rotations, default is 3-2-1 (aerospace body-to-inertial)
Returns:
A quaternion
"""
if not rads:
r = r2d(r)
p = r2d(p)
y = r2d(y)
return Quaternion.from_euler([r, p, y])
def Transform(lines, func):
for line in lines:
m = re.match(r'p f(\S+) w(\S+) h(\S+) v(\S+) (.*)', line)
if m:
f, w, h, v, post = m.groups()
w = int(w)
if w & 31:
w = (w + 31) & ~31
print 'Rounding size up to %d' % w
print 'Possible tile sizes: %d , %d , %d , etc.' % (w / 32, w / 16,
w / 8)
post = re.sub(' S[0-9,]+ ', ' ', post) # reset crop
yield 'p f0 w%d h%d v90 %s' % (w, w, post)
continue
m = re.match(r'i (.*) r(\S+) p(\S+) y(\S+) (.*)', line)
if m:
pre, r, p, y, post = m.groups()
r, p, y = func(d2r(float(r)), d2r(float(p)), d2r(float(y)))
yield 'i %s r%r p%r y%r %s' % (pre, r2d(r), r2d(p), r2d(y), post)
continue
yield line
def az_range(x,y,zoomlevel):
angle=180-r2d(atan2(x,y))
r=sqrt(x**2+y**2)/zoomlevel
return angle, r
def beamangle(h,R):
r=6371.0 #Earth radius
return r2d(asin(h/R-R/(2*1.21*r)))
import math math.r2d(math.pi)
