The program calculates the points of an end-effector with forward kinematics using the equation,
x = d1cos(a1) + d2cos(a1+a2)
y = d1sin(a1) + d2sin(a1+a2)
where d1 is the length of the first joint, d2 is the length of the second joint, a1 is the angle of the first joint and a2 is the angle of the second joint.
It calculates inverse kinematics by this equation
So, by entering the input required for forward kinematics I should get the points of the end effector. By entering in the same input and the points found in forward kinematics for inverse kinematics, I should get the angles I entered in as input for forward kinematics. But I do not get them back.
Here is my code,
'''
Created on Oct 5, 2015
#author: justin
'''
import math
def getOption():
print('Select an option:\n')
print('\t1) Forward Kinematics\n')
print('\t2) Inverse Kinematics\n')
option = input()
try:
option = int(option)
if option == 1:
fowardKinematics()
elif option == 2:
inverseKinematics()
else:
print('Not an option')
return
except ValueError:
print('Not an integer/Point cannot be reached')
return
def fowardKinematics():
'''
Ask user for input and computing points of end-effector
'''
length1 = input('Enter length of joint 1 (a1):\n') # Getting input from keyboard
angle1 = input('Enter angle of joint 1 (theta1):\n')
length2 = input('Enter length of joint 2 (a2):\n')
angle2 = input("Enter angle of join 2 (theta2)\n")
try:
length1 = float(length1) # Testing to see if user entered a number
length2 = float(length2) # Testing to see if user entered a number
angle1 = float(angle1) # Testing to see if user entered a number
angle2 = float(angle2) # Testing to see if user entered a number
except ValueError:
print('Invalid input, check your input again')
return
x = (length1 * math.cos(math.radians(angle1))) + (length2 * math.cos((math.radians(angle1 + angle2)))) # a1c1 + a2c12
y = (length1 * math.sin(math.radians(angle1))) + (length2 * math.sin((math.radians(angle1 + angle2)))) # a1s1 + a2s12
print('The position of the end-effector P(x,y) is:\n')
print('X: ' + str(x)) # Convert x to string
print('Y: ' + str(y)) # Convert y to string
def inverseKinematics():
length1 = input('Enter length of joint 1 (a1):\n')
length2 = input('Enter length of joint 2 (a2):\n')
x = input('Enter position of X:\n')
y = input('Enter position of Y:\n')
try:
length1 = float(length1)
length2 = float(length2)
x = float(x)
y = float(y)
except ValueError:
print('Invalid input, check your input again')
return
# Computing angle 2 Elbow up/down
numerator = ((length1 + length2)**2) - ((x**2) + (y**2))
denominator = ((x**2) + (y**2)) - ((length1 - length2)**2)
angle2UP = math.degrees(math.atan(math.sqrt(numerator/denominator)))
angle2DOWN = angle2UP * -1
# Angle 1 Elbow up
numerator = (length2 * math.sin(math.radians(angle2UP)))
denominator = ((length1 + length2) * math.cos(math.radians(angle2UP)))
angle1UP = math.degrees(math.atan2(numerator, denominator))
# Angle 1 Elbow down
numerator = (length2 * math.sin(math.radians(angle2DOWN)))
denominator = ((length1 + length2) * math.cos(math.radians(angle2DOWN)))
angle1DOWN = math.degrees(math.atan2(numerator, denominator))
print("Angle 1 Elbow up: " + str(angle1UP))
print("Angle 1 Elbow down: " + str(angle1DOWN))
print("Angle 2 Elbow up: " + str(angle2UP))
print("Angle 2 Elbow down: " + str(angle2DOWN))
if __name__ == '__main__':
getOption()
pass
I think the problem is when the trig functions get introduced. The parameters for them are supposed to be in radians, they return the answer is degrees. Somewhere I am mixing up the two. I just don't know where. Thanks
There is, I'm afraid, quite a bit wrong with this, either in your code or in the equations you're using. Your equation for theta2 doesn't make any sense to me if x and y are distances and a1 and a2 are angles (check your equation or give a source). Even if these should be d1 and d2, this equation involves subtracting two quantities with different dimensions (length^4 and length^2).
Then check your implementation of it, which does not evaluate the equation as given.
My advice about radians / degrees is to use radians throughout: accept the angles in degrees if you want, but then immediately convert to radians for the calculations, and convert angular results back to degrees for output.
Some more advice:
you don't need to cast your floats to strings for output using print, just use print('x: ', x) and so on.
Give your variables the same names as the symbols they represent in your formula. This would make it easier to debug (well, it would if the equations were correct).
Hope that helps.
Related
I decided to try and code ballistic computer. Below is the code i've put together that takes the muzzle velocity, the distance and the elevation bearing of a target and outputs the required angle to shoot so that a shell fired will collide at the desired location. Currently i'm suffering with an error in the second last line and i have no clue how to ammend this error. Any help at all would be appreciated.
import math
print("Note that the platform you are firing off must be perfectly flat to ensure optimal accuracy")
print("---------------------------------------------------------------")
g = (-9.81) **#g is negative in this case**
degrees = float(input("What is the angle from horizon of the spotter? [0-90] "))
radians = math.radians(degrees) **#Sin only works with radians**
U = float(input("What is the muzzle velocity of the gun? "))
Target_distance = float(input("What is the distance to the target in Meters? ")) #direct distance to target
y = float(math.sin(radians))**Target_distance #horizontal distance to target**
x = float(math.cos(radians))**Target_distance #elevation to target from you**
print("the elevation of the target is",y)
print("the distance to the targetenter code here is",x)
print("true distance to target is",Target_distance)
max_angle = math.radians(45)
max_dist = ((U**2)/(2*g))*(1+(math.sqrt(1+((2*g*y)/(U**2)*((math.sin(max_angle)*
(math.sin(max_angle)))))))*math.sin(max_angle))#shows the maximum distance of the shell being fired
print (("max gun range is"),-1*max_dist)
print("---------------------------------------------------------------")
theta = math.degrees((math.asin((g*x)/(U**2)))*-1) #equation needs to be halved to get correct solution
solution1 = (theta *0.5)
solution2 = (90 - solution1)
print(solution1)
print(solution2)
print("---------------------------------------------------------------")
#issue here (TypeError 'float' object is not callable) - variables passed in are U,g,x,y
"TypeError: 'float' object is not callable" for artillery script
solution_3 = math.degrees(math.atan((U*U) + (math.sqrt(U*U*U*U - g ( g * (x * x) + ( 2 * y * (U**2)))))) / ( g * x))
print (solution_3)
Use this instead:
solution_3 = math.degrees(math.atan((U**2) + (math.sqrt(U**4 - g * ( g * (x * x) + ( 2 * y * (U**2)))))) / ( g * x))
A * was missing after "g"
import math
g = -9.81
degrees = float(input("What is the angle from horizon of the spotter? [0-90] "))
radians = math.radians(degrees) #Sin only works with radians
U = int(input("What is the muzzle velocity of the gun? "))
Target_distance = float(input("What is the distance to the target in Meters? ")) #direct distance to target
y = float(math.sin(radians)) #Target_distance #horizontal distance to target
x = float(math.cos(radians)) #Target_distance #elevation to target from you
print("the elevation of the target is",y)
print("the distance to the targetenter code here is",x)
print("true distance to target is",Target_distance)
max_angle = math.radians(45)
max_dist = ((U*2)/(2*g))*(1+(math.sqrt(1+((2*g*y)/(U*2)*((math.sin(max_angle) * (math.sin(max_angle)))))))*math.sin(max_angle)) #shows the maximum distance of the shell being fired
print (("max gun range is"),-1*max_dist)
print("---------------------------------------------------------------")
theta = math.degrees((math.asin((g*x)/(U**2)))-1) #equation needs to be halved to get correct
# solution = ???
solution1 = (theta *0.5)
solution2 = (90 - solution1)
print(solution1)
print(solution2)
print("---------------------------------------------------------------")
The problem is that you forgot about sign between brackets * and varibles *
In python gU not working like g*U, you should specify what you want
I think I've found a common issue here:
...
max_dist = ((U2)/(2g))(1+(math.sqrt(1+((2gy)/(U2)((math.sin(max_angle)
... ^ ^ ^ ^ ^
| | | | |
You're getting a call error simply because you are not using the appropriate multiplication operator * for the variables I've pointed out above. This seems to be a common theme in your Python script as well.
It's great to see you implementing equations into code, and while you continue improving your programming skills you must look into organizing and structuring your code for legibility. That way you, the programming errors will be more apparent.
Im trying to get python to take a resultant value and angle value to return the original i and j values of a vector. I can get it to go from i and j values to resultant and angle but it is tricky the other way around.
#vector conversion (x,y) to resultant and angle#
import math
menu = input('Vector Conversion: for vector to (x,y) press 1. \n for (x,y) to vector press 2.')
if menu == '2':
user_distancex = float(input('What is the distance in x-direction?'))
user_distancey = float(input('What is the distance in y-direction?'))
r = (math.sqrt(user_distancex**2 + user_distancey**2))
theta = math.atan(user_distancey/user_distancex)*(180/math.pi)
print(r, 'feet',theta, 'degrees')
elif menu == '1':
user_angle = float(input('What is the angle of your vector?'))
user_resultant = float(input('What is the distance (resultant) of your vector'))
x_dist = user_resultant*math.cos(math.degrees(user_angle))
y_dist = user_resultant*math.sin(math.degrees(user_angle))
print((x_dist,y_dist))
math.degrees(user_angle) converts your user angle from radians to degrees. But it is already in degrees if the user inputs it in degrees!
You should use math.radians(user_angle) instead.
It also helps to clarify your input prompt:
user_angle = float(input('What is the angle of your vector (in degrees)?'))
I am trying to make a program that will find the x and y components of an applied force at an angle. Here is what I have so far. When I run it I get an error that basically says that you cannot take the cosine of a variable and that it has to be a real number. How would I make this type of program work?
Import math
Angle = input("Enter angle:")
Force = input("Enter applied force:")
X = math.cos(angle)
Y = x * force
Print("The x component of the applied force is", y)
B = math.cos(angle)
A = b * force
Print("The y component of the applied force is", A)
Here's code after fixing capitalization and changing type of inputted values to floats:
import math
angle = float(input("Enter angle:"))
force = float(input("Enter applied force:"))
x = math.cos(angle)
y = x * force
print("The x component of the applied force is", y)
b = math.cos(angle)
a = b * force
print("The y component of the applied force is", a)
In this program I've been working on with Python the goal is to take user inputs on a given initial velocity, angle, and how far away a structure is/how tall it is we're aiming for. I have been able to calculate how long it takes for something to reach a target, but I'm not sure why the final velocity (how fast it is going when it reaches the target) is coming up wrong.
# User inputs
velocity = float(input('Give me a velocity to fire at (in m/s): '))
angle = float(input('Give me an angle to fire at: '))
distance = float(input('Give me how far away you are from the
structure: '))
height = float(input('Give me the height of the structure (in meters):
'))
slingshot = 5 #Height of slingshot in meters
gravity = 9.8 #Earth gravity
# Converting angles to radians
angleRad = math.radians(angle)
# Computing our x and y coordinate
x = math.cos(angleRad)
y = math.sin(angleRad)
# Calculations
time = distance/(velocity * x)
vx = x
vy = y + (-9.8 * time)
finalVelocity = math.sqrt((vx ** 2) + (vy ** 2))
# Output of program
print('It takes your bird' , time , 'seconds to reach the structure')
print('Your velocity at the target distance is' , finalVelocity ,
'meters per second.')
Here is a sample input and what the expected output should be:
Input Velocity: 20
Input Angle: 40
Input Distance: 25
Input Height of Structure: 15
Expected Output:
Time to reach structure: 1.63176 s
Final velocity: 15.6384 s
My Program's Output:
Time to reach structure: 1.63176
Final velocity: 15.36755
At first glance it would appear my program is very close, so I suspected a rounding error, but it is mere coincidence with the chosen numbers that they're close.
You miscalculated the horizontal and vertical components of the final velocity. You only used the cosine and sine of the angle, rather than the (magnitude of the) initial velocity times the cosine and sine, respectively. If you modify the following two lines of code, you will obtain the result you were looking for given the sample input you provided:
vx = velocity * x
vy = velocity * y - 9.8 * time
I rewrote your original code a bit and also computed the final height to check whether the structure was hit or not, so feel free to use it if needed:
import math
# User inputs
# v0 = float(input('Give me a velocity to fire at (in m/s): '))
# angle = float(input('Give me an angle to fire at: '))
# distance = float(input('Give me how far away you are from the structure: '))
# height_structure = float(input('Give me the height of the structure (in meters):'))
# Test inputs
v0 = 20
angle = 40
distance = 25
height_structure = 15
# Constants
height_slingshot = 5 # Height of slingshot in meters
g = 9.8 # Earth gravity
# Converting angle to radians
angleRad = math.radians(angle)
# Computing initial velocity components
vx0 = v0 * math.cos(angleRad)
vy0 = v0 * math.sin(angleRad)
# Computing time to travel horizontal distance
t_x = distance / vx0
# Computing final vertical velocity component
vy_final = vy0 - g * t_x
# Computing magnitude of final velocity
v_final = math.sqrt((vx0 ** 2) + (vy_final ** 2))
# Note: Horizontal component is constant
# Computing final height
y_final = height_slingshot + vy0 * t_x - g / 2 * t_x ** 2
# Verify if t_x was computed correctly
# t_y1 = (vy0 + math.sqrt(vy0 ** 2 - 2 * g * y_final)) / g
# t_y2 = (vy0 - math.sqrt(vy0 ** 2 - 2 * g * y_final)) / g
# Output of program
print('It takes your bird', t_x, 'seconds to reach the structure.')
print('Your velocity at the target distance is', v_final,
'meters per second.')
print('\nFinal height: ', y_final)
print('Structure height:', height_structure)
if 0. <= y_final <= height_structure:
print('\nYou hit the structure!')
elif y_final < 0:
print('\nYou missed. Not far enough!')
else:
print('\nYou missed. Too far!')
I'm trying to write a program that will take a set of longitude & latitude coordinates from the user, convert them to x & y coordinates for a Mollweide projection map, and then report the value of the pixel at those coordinates (in this case, a noise temperature).
The map/data I'm using is the Haslam 408 MHz All Sky Survey which is provided as a Mollweide projection map. This data is in .fits format and is a large all-sky survey of noise in the 408 MHz band.
According to the Mollweide projection Wikipedia page, it is possible to use a Newton-Raphson iteration to convert from longitude/latitude to x/y map coordinates. I based the iteration scheme in my program largely on the methods from the Wikipedia page and in this GitHub post.
However, my program does not appear to be reporting the correct values for the longitude and latitude that I'm inputting. I largely suspect that one of two (or both) factors are contributing to this error:
The way that I'm implementing the iteration scheme is incorrect, and thus resulting in incorrect values being reported.
I don't properly understand what the radius value, R, represents in the iteration scheme. I can't find any literature on how to determine the proper R value beyond that "R is the radius of the globe to be projected." I assumed that this would be based upon the size of the map in pixels; in this case, the map image is 4096x2048 pixels, so I've tried using 2048, 1024, and simply 1 as the R values, to no avail.
Below I have provided my code for review:
from math import sin, cos, pi, sqrt, asin
from astropy.io import fits
hdulist = fits.open('data.fits')
hdulist.info()
data = hdulist[1].data
sqrt2 = sqrt(2)
def solveNR(lat, epsilon=1e-6): #this solves the Newton Raphson iteration
if abs(lat) == pi / 2:
return lat # avoid division by zero
theta = lat
while True:
nexttheta = theta - (
(2 * theta + sin(2 * theta) - pi * sin(lat)) /
(2 + 2 * cos(2 * theta))
)
if abs(theta - nexttheta) < epsilon:
break
theta = nexttheta
return nexttheta
def checktheta(theta, lat): #this function is also currently unused while debugging
return (2 * theta + sin(2 * theta), pi * sin(lat))
def mollweide(lat, lon, lon_0=0, R=1024):
lat = lat * pi / 180
lon = lon * pi / 180
lon_0 = lon_0 * pi / 180 # convert to radians
theta = solveNR(lat)
return (R * 2 * sqrt2 * (lon - lon_0) * cos(theta) / pi,
R * sqrt2 * sin(theta))
def inv_mollweide(x, y, lon_0=0, R=1024, degrees=True): # inverse procedure (x, y to lat, long). Currently unused
theta = asin(y / (R * sqrt2))
if degrees:
factor = 180 / pi
else:
factor = 1
return (
asin((2 * theta + sin(2 * theta)) / pi) * factor,
(lon_0 + pi * x / (2 * R * sqrt(2) * cos(theta))) * factor
)
def retrieve_temp(lat, long): #retrieves the noise temp from the data file after calling the mollweide function
lat = int(round(lat))
long = int(round(long))
coords = mollweide(lat, long)
x, y= coords
x = int(round(x))
y= int(round(y))
x = x-1
y = y-1
if x < 0:
x = x*(-1)
if y < 0:
y = y*(-1)
print("The noise temperature is: ",data[y, x],"K")
def prompt(): #this is the terminal UI
cont = 1
while cont == 1:
lat_cont = 1
while lat_cont == 1:
lat = float(input('Please enter the latitude: '))
lat_val = 1
while lat_val == 1:
if lat > 180 or lat < -180:
lat = float(input('Invalid input. Make sure your latitude value is in range -180 to 180 degrees \n'
'Please enter the latitude: '))
else:
lat_val = 0
lat_cont = 0
long_cont = 1
while long_cont == 1:
long = float(input('Please enter the longitude: '))
long_val = 1
while long_val == 1:
if long > 90 or long < -90:
long = float(input('Invalid input. Make sure your latitude value is in range -90 to 90 degrees \n'
'Please enter the latitude: '))
else:
long_val = 0
long_cont = 0
retrieve_temp(lat, long)
valid = 1
while valid == 1:
ans = input('Would you like to continue? Y or N: ').lower()
ans_val = 1
while ans_val ==1:
if not (ans == 'y' or ans == 'n'):
ans = input('Invalid input. Please answer Y or N to continue or exit: ')
elif ans == 'y':
ans_val = 0
cont = 1
valid = 0
elif ans == 'n':
ans_val = 0
cont = 0
valid = 0
prompt()
hdulist.close()
Apologies if I failed to follow typical Python conventions in the above code; I'm new to Python.
Your code looks reasonable. My advice about figuring out what's wrong:
(1) Try evaluating your mollweide and inv_mollweide functions at points for which you know what the results are supposed to be. E.g. points on the equator or prime meridian or something easy like that.
(2) Are your mollweide and inv_mollweide actually inverses? i.e. if you take your output from the one and put it into the other, you should get the original input back again.
(3) How do results change as you move around on the map? Do you get correct results in some areas (e.g. near middle of map) and not others? What happens as you get nearer to the edges? Does it gradually become more inaccurate or is there some threshold, beyond which you get grossly incorrect answers?
I think a feature of Newton's method is that it converges only if you're close enough to a solution to begin with, otherwise you can get anything. I don't know how close you have to be, for this problem.
This seems like a great problem. Good luck and have fun.
so this is going to sound really strange, but as far as I know there aren't actually any good Python libraries for Mollweide conversions, at least for Mollweide maps that span the entire earth. The EPSG system is bizarrely incomplete, which makes Pyproj more or less unusable for this purpose. I plan on using your code as part of a project of my own, but if you'd like to setup a github library and maybe even some kind of pip functionality (I'm fairly new to Python myself, so I'm really not who to ask on how to do that), I'm happy to give you credit.