Пример #1
0
	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
Пример #2
0
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)
Пример #3
0
	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
Пример #4
0
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])
Пример #5
0
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
Пример #6
0
def az_range(x,y,zoomlevel):
    angle=180-r2d(atan2(x,y))
    r=sqrt(x**2+y**2)/zoomlevel
    return angle, r
Пример #7
0
def beamangle(h,R):
    r=6371.0 #Earth radius
    return r2d(asin(h/R-R/(2*1.21*r)))
Пример #8
0
import math
math.r2d(math.pi)