I want to solve simple 2D inverse kinematic problem with Sympy. I know forward kinematic equation of x and y position.
x = l1*cos(theta1) + l2*cos(theta1+theta2)
y = l1*sin(theta1) + l2*sin(theta1+theta2)
How to solve theta1 and theta2 value with Sympy if I know those two equations?
I don't think there is a solution to those equations, but if there were you could use the follwing method:
import sympy as sp
# Define symbols
theta1, theta2, l1, l2, x, y = sp.symbols("theta1 theta2 l1 l2 x y")
# Define equations, rearranged so expressions equal 0
eq1 = l1 * sp.cos(theta1) + l2 * sp.cos(theta1 + theta2) - x
eq2 = l1 * sp.sin(theta1) + l2 * sp.sin(theta1 + theta2) - y
# Solve for theta1 & theta2
solution = sp.solve([eq1, eq2], [theta1, theta2], dict=True)
print(solution)
I was trying to use the sympy nonlinsolve solver for a similar inverse kinematics problem but noticed this comment in the docs:
Currently nonlinsolve is not properly capable of solving the system of equations having trigonometric functions. solve can be used for such cases (but does not give all solution)
Related
I'm writing a function that takes in an object with a trajectory (including starting position, starting velocity, and acceleration, all represented as Vector3s) in 3D space and if it hits another object, returns the point of collision and time of the collision. I'm using kinematic equations with a timestep to detect possible collisions and I can get the point of collision that way, but once I have that I want to find the exact time that that collision would occur at.I thought of rearranging a kinematic equation to solve for time and plug in what I already had, but I can't figure out how I can use all three axes of motion to do this, since my other values are Vec3's and time is just scalar. I've thought about just doing the calculation on one axis, but I'm not sure if that would lead to an accurate result.
Would it be accurate to calculate just based on one axis, or is there a way to incorporate all three into the calculation? The formula I'm using to solve for time is:
t = (v_init +/- Sqrt((v_init)^2 - (accel * disp * 4 * .5)))/accel;
Where v_init is initial velocity, disp is total displacement, and accel is acceleration. I'm basing this off of the kinematic equation:
d = v*t + .5*a*t^2
Let me write in the general case. The component-wise motion law is
x(t) = x0 + v_x t + 0.5 a_x t^2
y(t) = y0 + v_y t + 0.5 a_y t^2
z(t) = z0 + v_z t + 0.5 a_z t^2
where (x0,y0,z0)^t is the initial position, (v_x, v_y, v_z)^t is the initial velocity vector, and (a_x, a_y, a_z)^t is the vector of acceleration. The 3rd component of the latter may include also the gravity acceleration.
I assume that the collision plane is horizontal, having thus equation z = k. Solve in t the equation
z(t) = k
for finding the time t_c in which the projectile hits the plane. Compute then the collision coordinates x(t_c) and y(t_c) using the above formula by substituting t with t_c.
If the plane has the general equation
a x + b y +c z + d = 0
I suggest to put the frame of reference on the plane, having the xy plane on the collision plane, and then apply the above procedure.
You may also solve the non linear system
x = x0 + v_x t + 0.5 a_x t^2
y = y0 + v_y t + 0.5 a_y t^2
z = z0 + v_z t + 0.5 a_z t^2
a x + b y +c z + d = 0
taking the solution for t>0 (I dropped the dependency on t for x, y and z).
To solve it in C++, you may search a math library, such as Eigen which has a module for non linear systems.
I'm trying to use sympy to generate equations for non-linear least squares fitting. My goal is to make this quite complex but for the moment, here's a simple case (but not too simple!). It's basically fitting a two dimensional sinusoid to data. Here's the sympy code:
from sympy import *
S, l, m = symbols('S l m', real=True)
u, v = symbols('u v', real=True)
Vobs = symbols('Vobs', complex=True)
Vres = Vobs - S * exp(- 1j * 2 * pi * (u*l+v*m))
J=Vres*conjugate(Vres)
axes = [S, l, m]
grad = derive_by_array(J, axes)
hess = derive_by_array(grad, axes)
One element of the grad term looks like:
- 2.0*I*pi*S*u*(-S*exp(-2.0*I*pi*(l*u + m*v)) + Vobs)*exp(2.0*I*pi*(l*u + m*v)) + 2.0*I*pi*S*u*(-S*exp(2.0*I*pi*(l*u + m*v)) + conjugate(Vobs))*exp(-2.0*I*pi*(l*u + m*v))
What I'd like is to replace the expanded term (-S*exp(-2.0*I*pi*(l*u + m*v)) + Vobs) by Vres and contract the two conjugate terms into the more compact equivalent is:
4.0*pi*S*u*im(Vres*exp(2.0*I*pi*(l*u + m*v)))
I cannot see how to do this with sympy. This problem is bad for the first derivative (grad) but get really out of hand with the second derivative (hess).
First of all, let's not use 1j in SymPy, it's a float and floats are bad for symbolic math. SymPy's imaginary unit is I. So,
Vres = Vobs - S * exp(- I * 2 * pi * (u*l+v*m))
To replace the expression Vres by a symbol, we first need to create such a symbol. I'm going to call it Vres0, but its name will be Vres, so it prints as "Vres" in formulas.
Vres0 = symbols('Vres')
g1 = grad[1].subs(Vres, Vres0).conjugate().subs(Vres, Vres0).conjugate()
The conjugate-substitute-conjugate back is needed because subs doesn't quite recognize the possibility of replacing the conjugate of an expression with the conjugate of the symbol.
Now g1 is
-2*I*pi*S*Vres*u*exp(2*I*pi*(l*u + m*v)) + 2*I*pi*S*u*exp(-2*I*pi*(l*u + m*v))*conjugate(Vres)
and we want to fold the sum of conjugate terms. I use a custom transformation rule for this: the rule fold_conjugates applies to every sum (Add) of two terms (len(f.args) == 2) where the second is a conjugate of the first (f.args[1] == f.args[0].conjugate()). The transformation it performs: replace the sum by twice the real part of first argument (2*re(f.args[0])). Like so:
from sympy.core.rules import Transform
fold_conjugates = Transform(lambda f: 2*re(f.args[0]),
lambda f: isinstance(f, Add) and len(f.args) == 2 and f.args[1] == f.args[0].conjugate())
g = g1.xreplace(fold_conjugates)
Final result: 4*pi*S*u*im(Vres*exp(2*I*pi*(l*u + m*v))).
I would like to fit a 2D array by an elliptic function: (x / a)² + (y / b)² = 1 ----> (and so get the a and b)
And then, be able to replot it on my graph.
I found many examples on internet, but no one with this simple Cartesian equation. I probably have searched badly ! I think a basic solution for this problem could help many people.
Here is an example of the data:
Sadly, I can not put the values... So let's assume that I have an X,Y arrays defining the coordinates of each of those points.
This can be solved directly using least squares. You can frame this as minimizing the sum of squares of quantity (alpha * x_i^2 + beta * y_i^2 - 1) where alpha is 1/a^2 and beta is 1/b^2. You have all the x_i's in X and the y_i's in Y so you can find the minimizer of ||Ax - b||^2 where A is an Nx2 matrix (i.e. [X^2, Y^2]), x is the column vector [alpha; beta] and b is column vector of all ones.
The following code solves the more general problem for an ellipse of the form Ax^2 + Bxy + Cy^2 + Dx +Ey = 1 though the idea is exactly the same. The print statement gives 0.0776x^2 + 0.0315xy+0.125y^2+0.00457x+0.00314y = 1 and the image of the ellipse generated is also below
import numpy as np
import matplotlib.pyplot as plt
alpha = 5
beta = 3
N = 500
DIM = 2
np.random.seed(2)
# Generate random points on the unit circle by sampling uniform angles
theta = np.random.uniform(0, 2*np.pi, (N,1))
eps_noise = 0.2 * np.random.normal(size=[N,1])
circle = np.hstack([np.cos(theta), np.sin(theta)])
# Stretch and rotate circle to an ellipse with random linear tranformation
B = np.random.randint(-3, 3, (DIM, DIM))
noisy_ellipse = circle.dot(B) + eps_noise
# Extract x coords and y coords of the ellipse as column vectors
X = noisy_ellipse[:,0:1]
Y = noisy_ellipse[:,1:]
# Formulate and solve the least squares problem ||Ax - b ||^2
A = np.hstack([X**2, X * Y, Y**2, X, Y])
b = np.ones_like(X)
x = np.linalg.lstsq(A, b)[0].squeeze()
# Print the equation of the ellipse in standard form
print('The ellipse is given by {0:.3}x^2 + {1:.3}xy+{2:.3}y^2+{3:.3}x+{4:.3}y = 1'.format(x[0], x[1],x[2],x[3],x[4]))
# Plot the noisy data
plt.scatter(X, Y, label='Data Points')
# Plot the original ellipse from which the data was generated
phi = np.linspace(0, 2*np.pi, 1000).reshape((1000,1))
c = np.hstack([np.cos(phi), np.sin(phi)])
ground_truth_ellipse = c.dot(B)
plt.plot(ground_truth_ellipse[:,0], ground_truth_ellipse[:,1], 'k--', label='Generating Ellipse')
# Plot the least squares ellipse
x_coord = np.linspace(-5,5,300)
y_coord = np.linspace(-5,5,300)
X_coord, Y_coord = np.meshgrid(x_coord, y_coord)
Z_coord = x[0] * X_coord ** 2 + x[1] * X_coord * Y_coord + x[2] * Y_coord**2 + x[3] * X_coord + x[4] * Y_coord
plt.contour(X_coord, Y_coord, Z_coord, levels=[1], colors=('r'), linewidths=2)
plt.legend()
plt.xlabel('X')
plt.ylabel('Y')
plt.show()
Following the suggestion by ErroriSalvo, here is the complete process of fitting an ellipse using the SVD. The arrays x, y are coordinates of the given points, let's say there are N points. Then U, S, V are obtained from the SVD of the centered coordinate array of shape (2, N). So, U is a 2 by 2 orthogonal matrix (rotation), S is a vector of length 2 (singular values), and V, which we do not need, is an N by N orthogonal matrix.
The linear map transforming the unit circle to the ellipse of best fit is
sqrt(2/N) * U * diag(S)
where diag(S) is the diagonal matrix with singular values on the diagonal. To see why the factor of sqrt(2/N) is needed, imagine that the points x, y are taken uniformly from the unit circle. Then sum(x**2) + sum(y**2) is N, and so the coordinate matrix consists of two orthogonal rows of length sqrt(N/2), hence its norm (the largest singular value) is sqrt(N/2). We need to bring this down to 1 to have the unit circle.
N = 300
t = np.linspace(0, 2*np.pi, N)
x = 5*np.cos(t) + 0.2*np.random.normal(size=N) + 1
y = 4*np.sin(t+0.5) + 0.2*np.random.normal(size=N)
plt.plot(x, y, '.') # given points
xmean, ymean = x.mean(), y.mean()
x -= xmean
y -= ymean
U, S, V = np.linalg.svd(np.stack((x, y)))
tt = np.linspace(0, 2*np.pi, 1000)
circle = np.stack((np.cos(tt), np.sin(tt))) # unit circle
transform = np.sqrt(2/N) * U.dot(np.diag(S)) # transformation matrix
fit = transform.dot(circle) + np.array([[xmean], [ymean]])
plt.plot(fit[0, :], fit[1, :], 'r')
plt.show()
But if you assume that there is no rotation, then np.sqrt(2/N) * S is all you need; these are a and b in the equation of the ellipse.
You could try a Singular Value Decomposition of the data matrix.
https://docs.scipy.org/doc/numpy-1.13.0/reference/generated/numpy.linalg.svd.html
First center the data by subtracting mean values of X,Y from each column respectively.
X=X-np.mean(X)
Y=Y-np.mean(Y)
D=np.vstack(X,Y)
Then, apply SVD and extract
-eigenvalues (members of s) -> axis length
-eigenvectors(U) -> axis orientation
U, s, V = np.linalg.svd(D, full_matrices=True)
This should be a least-squares fit.
Of course, things can get more complicated than this, please see
https://www.emis.de/journals/BBMS/Bulletin/sup962/gander.pdf
I am trying to solve something which may not actually be too complicated (mathematically), but I'm a bit lost and want to do this wich SymPy's aid. I
Given are two variables X, Y (these are symbolic, I do not have access to any values) and known constants a, b, ..., g. In a piece of existing software that I cannot change, from these
X' = a * X + b
Y' = c * Y + d
Z' = e * X * Y + f
are calculated. In another different piece of existing software that I cannot change,
I have different values, a similar calculation is done. I am greatly simplifying this for purposes of this question, but to get the idea:
X' and Y' like above
Z_ = (a * x + b) * ( c * y + d) - g
To bridge between Z' and Z_, I want to express z_ as a linear combination of X', Y', Z', and 1:
Z_ == 𝛂 * X' + 𝛃 * Y' + 𝛄 * Z' + 𝛅.
How can I solve this for 𝛂, 𝛃, 𝛄, 𝛅 with Sympy?
I know I could probably avoid Sympy, expressing each of the variables X', Y', Z', Z_ as linear combinations of X, Y, X*Y, and 1, and doing matrix algebra But since my problem actually is more complicated and its details likely to change in the future, I would really appreciate some software assistance.
I need a method that allows me to find the Y-coordinate on a Cubic Bezier Curve, given an x-coordinate.
I've come across lots of places telling me to treat it as a cubic function then attempt to find the roots, which I understand. HOWEVER the equation for a Cubic Bezier curve is (for x-coords):
X(t) = (1-t)^3 * X0 + 3*(1-t)^2 * t * X1 + 3*(1-t) * t^2 * X2 + t^3 * X3
What confuses me is the addition of the (1-t) values. For instance, if I fill in the X values with some random numbers:
400 = (1-t)^3 * 100 + 3*(1-t)^2 * t * 600 + 3*(1-t) * t^2 * 800 + t^3 * 800
then rearrange it:
800t^3 + 3*(1-t)*800t^2 + 3*(1-t)^2*600t + (1-t)^3*100 -400 = 0
I still don't know the value of the (1-t) coefficients. How I am I supposed to solve the equation when (1-t) is still unknown?
There are three common ways of expressing a cubic bezier curve.
First x as a function of t
x(t) = sum( f_i(t) x_i )
= (1-t)^3 * x0 + 3*(1-t)^2 * t * x1 + 3*(1-t) * t^2 * x2 + t^3 * x3
Secondly y as a function of x
y(x) = sum( f_i(x) a_i )
= (1-x)^3 * y0 + 3*(1-x)^2 * x * y1 + 3*(1-x) * x^2 * y2 + x^3 * y3
These first two are mathematically the same, just using different names for the variables.
Judging by your description "find the Y-coordinate on a Cubic Bezier Curve, given an x-coordinate on it." I'm guessing that you've got a question using the second equation are are trying to rearrange the first equation to help you solve it, where as you should be using the second equation. If thats the case, then no rearranging or solving is required - just plug your x value in and you have the solution.
Its possible that you have an equation of the third kind case, which is the ugly and hard case.
This is both the x and y parameters are cubic Beziers of a third variable t.
x(t) = sum( f_i(t) x_i )
y(t) = sum( f_i(t) y_i )
If this is your case. Let me know and I can detail what you need to do to solve it.
I think this is a fair CS question, so I'm going to attempt to show how I solved this. Note that a given x may have more than 1 y value associated with it. In the case where I needed this, that was guaranteed not to be the case, so you'll have to figure out how to determine which one you want.
I iterated over t generating an array of x and y values. I did it at a fairly high resolution for my purposes. (I was looking to generate an 8-bit look-up table, so I used ~1000 points.) I just plugged t into the bezier equation for the next x and the next y coordinates to store in the array. Once I had the entire thing generated, I scanned through the array to find the 2 nearest x values. (Or if there was an exact match, used that.) I then did a linear interpolation on that very small line segment to get the y-value I needed.
Developing the expression further should get you rid of the (1 - t) factors
If you run:
expand(800*t^3 + 3*(1-t)*800*t^2 + 3*(1-t)^2*600*t + (1-t)^3*100 -400 = 0);
In either wxMaxima or Maple (you have to add the parameter t in this one though), you get:
100*t^3 - 900*t^2 + 1500*t - 300 = 0
Solve the new cubic equation for t (you can use the cubic equation formula for that), after you got t, you can find x doing:
x = (x4 - x0) * t (asuming x4 > x0)
Equation for Bezier curve (getting x value):
Bx = (-t^3 + 3*t^2 - 3*t + 1) * P0x +
(3*t^3 - 6*t^2 + 3*t) * P1x +
(-3*t^3 + 3*t^2) * P2x +
(t^3) * P3x
Rearrange in the form of a cubic of t
0 = (-P0x + 3*P1x - 3*P2x + P3x) * t^3+
(3*P0x - 6*P1x + 3*P2x) * t^2 +
(-3*P0x + 3*P1x) * t +
(P0x) * P3x - Bx
Solve this using the cubic formula to find values for t. There may be multiple real values of t (if your curve crosses the same x point twice). In my case I was dealing with a situation where there was only ever a single y value for any value of x. So I was able to just take the only real root as the value of t.
a = -P0x + 3.0 * P1x - 3.0 * P2x + P3x;
b = 3.0 * P0x - 6.0 * P1x + 3.0 * P2x;
c = -3.0 * P0x + 3.0 * P1x;
d = P0x;
t = CubicFormula(a, b, c, d);
Next put the value of t back into the Bezier curve for y
By = (1-t)^3 * P0x +
3t(1-t)^2 * P1x +
3t^2(1-t) * P2x +
t^3 * P3x
So I've been looking around for some sort of method to allow me to find the Y-coordinate on a Cubic Bezier Curve, given an x-coordinate on it.
Consider a cubic bezier curve between points (0, 0) and (0, 100), with control points at (0, 33) and (0, 66). There are an infinite number of Y's there for a given X. So there's no equation that's going to solve Y given X for an arbitrary cubic bezier.
For a robust solution, you'll likely want to start with De Casteljau's algorithm
Split the curve recursively until individual segments approximate a straight line. You can then detect whether and where these various line segments intercept your x or whether they are vertical line segments whose x corresponds to the x you're looking for (my example above).