Sympy.solve can't eliminate intermediate symbols from systems of equations
WHAT I WANT TO DO
I am building a DIY robot arm. I need to design the length of the links, the joint configuration, and motors size function of my working area and payload.
I decided it would be a fun project to make a program to help me with the task as it would help simulating the reach, and finding the forward and inverse kinematic matrices that control the robot.
I want the python program to be parametric, so i can define how many motors and links are present, how they are connected, and I want the python program to do three things:
- Plot me the reach with given parameters
- Find me the sensitivities of the end effector position in respect to the motor angles
- Compile the forward and inverse transformation matricies with given parameters
It's very simple to solve manually for two motors and two links, I want to try configurations and generally have fun with shapes and working areas, that's why I'm trying to make a more generic solver. For reference, this is the kind of construction that I want the program to provide the equations for:
WHERE I GOT SO FAR
I made a python program using the Sympy library to replicate what I do on paper: I define the equations of the mechanical system, individual joints and links and I get a system of equations, then use Sympy to solve the equations, both forward and reverse, scan and plot the position over given input angle range. It demonstrates that the Sympy solver can do what I need, solving a two joint system:
import numpy as lib_numpy
# Import sympy library
import sympy as lib_sympy
#Display
import matplotlib.pyplot as plt
#create a symbolic 2D rotation matrix (2x2)
def create_rotation_matrix_2d( theta : lib_sympy.Symbol):
return lib_sympy.Matrix([[lib_sympy.cos(theta), -lib_sympy.sin(theta)], [lib_sympy.sin(theta), lib_sympy.cos(theta)]])
#create a symbolic 2D vector 2x1 vertical x, y
def create_vector_2d( x : lib_sympy.Symbol, y : lib_sympy.Symbol ):
return lib_sympy.Matrix([[x, y]])
#A segment is composed by a joint connected to a link
def create_segment( in_phi : lib_sympy.Symbol, in_theta : lib_sympy.Symbol, in_length : lib_sympy.Symbol ):
#joints
nnn_rotation_matrix = create_rotation_matrix_2d( in_phi +in_theta )
#link from this joint to the next joint to come
nn_link_vector = create_vector_2d( in_length, 0 )
#equation of the link
return nn_link_vector *nnn_rotation_matrix
def system():
#---------------------------------------------------
# WORLD - J1
#---------------------------------------------------
eq_link_w_1 = create_segment( 0, 0, 0 )
#---------------------------------------------------
# J1 - J2
#---------------------------------------------------
n_theta_1 = lib_sympy.Symbol('T1')
n_length_1_2 = lib_sympy.Symbol('L1E')
eq_link_1_2 = create_segment( 0, n_theta_1, n_length_1_2 )
#---------------------------------------------------
# J2 - EE
#---------------------------------------------------
n_theta_2 = lib_sympy.Symbol('T2')
n_length_2_e = lib_sympy.Symbol('L2E')
eq_link_2_e = create_segment( 0, n_theta_1 +n_theta_2, n_length_2_e )
#---------------------------------------------------
# END EFFECTOR
#---------------------------------------------------
#spawn the output of the system (referred to world)
n_x = lib_sympy.Symbol('x')
n_y = lib_sympy.Symbol('y')
nn_end_effector = create_vector_2d( n_x, n_y )
#---------------------------------------------------
# EQUATION
#---------------------------------------------------
#build the equation
eq = lib_sympy.Eq( nn_end_effector, eq_link_w_1 +eq_link_1_2 +eq_link_2_e )
print("Equation: ", eq )
solution_forward = lib_sympy.solve( eq, [n_x, n_y], dict = True )
print("num solutions: ", len(solution_forward))
print("Forward Solution theta->x,y: ", solution_forward )
#Construct equations from solutions
for n_solution_index in solution_forward: #loop over the solutions
#equation from solution so I can substitute
equation_forward_x = lib_sympy.Eq( n_x, n_solution_index[n_x] )
equation_forward_y = lib_sympy.Eq( n_y, n_solution_index[n_y] )
print("Equation X: ", equation_forward_x )
print("Equation Y: ", equation_forward_y )
#---------------------------------------------------
# MAP THE REACH
#---------------------------------------------------
# I initialize a "result" array. it contains the X and Y output
# I decide the minimum and maximum angle range of the joints
# for each solution
# first I set the length of the link, a parameter
# I replace the angle of the joints in the equation
# I solve the equation, and record the result
#I initialize a “result” array. it contains the X and Y output
result = []
#samples
n_steps = 8
#Angles J1
min = -lib_numpy.pi/2
max = lib_numpy.pi/2
nn_angles_j1 = lib_numpy.linspace( min, max, n_steps )
#Angles J2
min = -lib_numpy.pi/2
max = lib_numpy.pi/2
nn_angles_j2 = lib_numpy.linspace( min, max, n_steps )
#link length
in_length_1_2 = 5
in_length_2_e = 3
#scan angles for J1
for n_angle_j1 in nn_angles_j1:
for n_angle_j2 in nn_angles_j2:
#I replace the angle of the joints in the equation
n_eval_x = equation_forward_x.evalf( subs={n_theta_1: n_angle_j1, n_theta_2 : n_angle_j2, n_length_1_2 : in_length_1_2, n_length_2_e : in_length_2_e } )
sol_x = lib_sympy.solve( n_eval_x, dict=True )
#print("solution X: ", sol_x )
n_eval_y = equation_forward_y.evalf( subs={n_theta_1: n_angle_j1, n_theta_2 : n_angle_j2, n_length_1_2 : in_length_1_2, n_length_2_e : in_length_2_e } )
sol_y = lib_sympy.solve( n_eval_y, dict=True )
#print("solution Y: ", sol_y )
sol = [ sol_x[0][n_x], sol_y[0][n_y] ]
#solve the now simple equation and get a number
#print("solution: ", sol )
#I solve the equation, and record the result
result.append( sol )
#---------------------------------------------------
# PRINT XY
#---------------------------------------------------
# Print a scatter XY chart showing all the X,Y points
#---------------------------------------------------
#Print a scatter XY chart showing all the X,Y points
x_values = [r[0] for r in result]
y_values = [r[1] for r in result]
plt.scatter(x_values, y_values)
plt.xlabel("x")
plt.ylabel("y")
plt.title("Reachable points for a single link")
plt.show()
#if execution detected
if __name__ == '__main__':
system()
Output:
Equation: Eq(Matrix([[x, y]]), Matrix([[L1E*cos(T1) + L2E*cos(T1 + T2), -L1E*sin(T1) - L2E*sin(T1 + T2)]]))
num solutions: 1
Forward Solution theta->x,y: [{x: L1E*cos(T1) + L2E*cos(T1 + T2), y: -L1E*sin(T1) - L2E*sin(T1 + T2)}]
Equation X: Eq(x, L1E*cos(T1) + L2E*cos(T1 + T2))
Equation Y: Eq(y, -L1E*sin(T1) - L2E*sin(T1 + T2))
Forward End Effector XY map for some T1 T2 joint configuration
PROBLEM
Sympy only seems to be able to solve if there are no intermediate vars, which is baffling to me.
If I provide the full system of equations, Sympy.solve seems unable to see the equations M1X = 0 and M2X = M1X and eliminate the M1X symbol, even if I add it to the exclude list of the solver.
E.g. The following code:
import numpy as lib_numpy
# Import sympy library
import sympy as lib_sympy
#Display
import matplotlib.pyplot as plt
def create_rotation_matrix_2d( theta : lib_sympy.Symbol):
return lib_sympy.Matrix([[lib_sympy.cos(theta), lib_sympy.sin(theta)], [-lib_sympy.sin(theta), lib_sympy.cos(theta)]])
def create_vector_2d( x : lib_sympy.Symbol, y : lib_sympy.Symbol ):
return lib_sympy.Matrix([[x, y]])
def create_segment( in_phi : lib_sympy.Symbol, in_theta : lib_sympy.Symbol, in_length : lib_sympy.Symbol ):
#joints
nnn_rotation_matrix = create_rotation_matrix_2d( in_phi +in_theta )
#link from this joint to the next joint to come
nn_link_vector = create_vector_2d( in_length, 0 )
#equation of the link
return nn_link_vector *nnn_rotation_matrix
def create_segment_offset( in_start_x : lib_sympy.Symbol, in_start_y : lib_sympy.Symbol, in_phi : lib_sympy.Symbol, in_theta : lib_sympy.Symbol, in_length : lib_sympy.Symbol ):
nn_offset = create_vector_2d( in_start_x, in_start_y )
nn_segment = create_segment( in_phi, in_theta, in_length )
return nn_offset +nn_segment
def create_segment_equations( in_length : lib_sympy.Symbol, in_start_x : lib_sympy.Symbol, in_start_y : lib_sympy.Symbol, in_phi : lib_sympy.Symbol, in_theta : lib_sympy.Symbol, in_end_x : lib_sympy.Symbol, in_end_y : lib_sympy.Symbol, in_end_theta : lib_sympy.Symbol ):
l_equation = []
#Segment X,Y equations function of angle
equation_1 = lib_sympy.Eq( create_vector_2d( in_end_x, in_end_y ), create_segment_offset( in_start_x, in_start_y, in_phi, in_theta, in_length) )
solution_1 = lib_sympy.solve( [equation_1], [in_end_x])
solution_2 = lib_sympy.solve( [equation_1], [in_end_y])
#Segment T angle equation function of angle
equation_theta = lib_sympy.Eq( in_end_theta, in_phi+in_theta )
#compose segment equations
l_equation.append( lib_sympy.Eq( in_end_x, solution_1[in_end_x] ) )
l_equation.append( lib_sympy.Eq( in_end_y, solution_2[in_end_y] ) )
l_equation.append( equation_theta )
return l_equation
def double_pendulum_system():
#forward equations
#T1,T2->EX,EY,ET
l_equation = []
#Motor 1 segment from World to its joint
n_segment_1_length = lib_sympy.Symbol('L1')
n_motor_1_theta = lib_sympy.Symbol('T1')
n_segment_1_x = lib_sympy.Symbol('M2X')
n_segment_1_y = lib_sympy.Symbol('M2Y')
n_segment_1_theta = lib_sympy.Symbol('M2T')
l_equation = l_equation +create_segment_equations( n_segment_1_length, 0, 0, 0, n_motor_1_theta, n_segment_1_x, n_segment_1_y, n_segment_1_theta )
#Motor 2 segment from Motor 1 Joint to End Effector
n_segment_2_length = lib_sympy.Symbol('L2')
n_motor_2_theta = lib_sympy.Symbol('T2')
n_end_effector_x = lib_sympy.Symbol('EX')
n_end_effector_y = lib_sympy.Symbol('EY')
n_end_effector_theta = lib_sympy.Symbol('ET')
l_equation = l_equation +create_segment_equations( n_segment_1_length, n_segment_1_x, n_segment_1_y, n_segment_1_theta, n_motor_2_theta, n_end_effector_x, n_end_effector_y, n_end_effector_theta )
print( "Equations", l_equation )
#Forward Equation
l_forward_solution = lib_sympy.solve( l_equation, [n_end_effector_x, n_end_effector_y, n_end_effector_theta],exclude=(n_segment_1_x,n_segment_1_y,n_segment_1_y,) )
print( "Forward", l_forward_solution )
#Forward Sensitivity
#Sensitivity of End Effector X in respect to variations in T1 angle
n_end_effector_sensitivity_x_t1 = lib_sympy.Symbol('EXdT1')
l_equation.append( lib_sympy.Eq( n_end_effector_sensitivity_x_t1, lib_sympy.Derivative(n_end_effector_x, n_motor_1_theta) ) )
l_sensitivity = lib_sympy.solve( l_equation, [n_end_effector_sensitivity_x_t1] )
print("Forward Sensitivity", l_sensitivity )
return
if __name__ == '__main__':
print("TEST12")
print("Forward density chart")
#STEP1: compile forward equations
double_pendulum_system()
#STEP2: compile forward sensitivity, how sensitive is position to T1 and T2
unhelpful outputs:
TEST12
Forward density chart
Equations [Eq(M2X, L1*cos(T1)), Eq(M2Y, L1*sin(T1)), Eq(M2T, T1), Eq(EX, L1*cos(M2T + T2) + M2X), Eq(EY, L1*sin(M2T + T2) + M2Y), Eq(ET, M2T + T2)]
Forward {EX: L1*cos(M2T + T2) + M2X, EY: L1*sin(M2T + T2) + M2Y, ET: M2T + T2}
Forward Sensitivity {EXdT1: Derivative(EX, T1)}
Which tells me that the derivative is the derivative, which is unhelpful to say the least. The correct output I want to get is like:
TEST12
Forward density chart
Equations [Eq(M2X, L1*cos(T1)), Eq(M2Y, L1*sin(T1)), Eq(M2T, T1), Eq(EX, L1*cos(M2T + T2) + M2X), Eq(EY, L1*sin(M2T + T2) + M2Y), Eq(ET, M2T + T2)]
Forward {EX: L1*cos(T1+ T2) + L1*cos(T1), EY: L1*sin(T1+ T2) + L1*sin(T1), ET: T1+ T2}
Forward Sensitivity {EXdT1: -L1*sin(T1)-L1*sin(T1+ T2)}
I tried linsolve to no avail, the equations are inherently trascendent, linsolve can't do anything with them.
QUESTION
- How can I get Sympy.solve to sobstitute/eliminate unneded variables and get it to give me a closed solution?
- is there anothe Sympy call or another way to properly symbolically solve the system of equations I provided in the example?
below a minimal snippet to replicate the problem in Sympy.solve
#Try to solve X^2=Y+ReplaceMe for X with ReplaceMe=-1
import sympy as lib_sympy
def good( in_x : lib_sympy.Symbol, in_y : lib_sympy.Symbol, in_z : lib_sympy.Symbol ):
equation = lib_sympy.Eq( in_x*in_x, in_y+ in_z )
equation = equation.evalf( subs={in_z: -1} )
solution = lib_sympy.solve( equation, in_x )
return solution
#The solver doesn't realize that Z=0 and doesn't sobstitute
def bad( in_x : lib_sympy.Symbol, in_y : lib_sympy.Symbol, in_z : lib_sympy.Symbol ):
l_equation = []
l_equation.append( lib_sympy.Eq( in_z, -1 ) )
l_equation.append( lib_sympy.Eq( in_x*in_x, in_y+ in_z ) )
solution = lib_sympy.solve( l_equation, in_x, exclude = (in_z,) )
return solution
if __name__ == '__main__':
n_x = lib_sympy.Symbol('X')
n_y = lib_sympy.Symbol('Y')
n_z = lib_sympy.Symbol('ReplaceMe')
print("Manual Solution: ", good( n_x, n_y, n_z ) )
print("Unhelpful Solution: ", bad( n_x, n_y, n_z ) )
Output:
Manual Solution: [-sqrt(Y - 1.0), sqrt(Y - 1.0)]
Unhelpful Solution: [(-sqrt(ReplaceMe + Y),), (sqrt(ReplaceMe + Y),)]
Solution
First off, when you call solve, you must tell it what variables to solve for. i.e. if you tell it to only solve for x, it will only solve for x -- and not for any intermediate variables...
There are a few things you can attempt. If you have a simple expression, such as you do in your toy example, you can simply use Eq.subs to substitute into your equation :
from sympy import symbols, Eq, solve
x, y, z = symbols('x y z')
equation = Eq(x*x, y + z)
equation = equation.subs(z, -1) # do the simple substitution
solution = solve(equation, x) # this equation has 2 variables, so we try to solve for one as a function of the other
print(solution)
# >>> [-sqrt(y - 1), sqrt(y - 1)]
otherwise, the more general approach is to solve for all your variables :
from sympy import symbols, Eq, solve
x, y, z = symbols('x y z')
equations = [
Eq(x*x, y + z),
Eq(z, -1)
]
solution = solve(equations, (x, z)) # this equation has 3 variables, so we try to solve for one as a function of the TWO others
print(solution)
# >>> [(-sqrt(y - 1), -1), (sqrt(y - 1), -1)]
This will output the value of your intermediate variables in the solution, but it's pretty trivial to strip it out with some post-processing.
Final extremely pedantic not : please follow usual conventions when writing your code. it makes reading and understanding your code much simpler for others (i.e. import libraries by their standard names, call symbolic variables by the same name as the code variables)
- RuntimeError: Trying to backward through the graph a second time on loss tensor
- ERROR: post() got an unexpected keyword argument 'proxies'
- How do I automatically fix an invalid JSON string?
- Python generating a list of dates between two dates
- ModuleNotFoundError: No module named 'json'
- Check if string is None, empty or has spaces only
- invalid command name ".!canvas"
- SqlAlchemy: How to make a LONGBLOB column in mysql?
- Validate if input number is prime
- OpenAI remove key access while using AAD authentication
- Converting a Text File to a String in Python
- Problems with repeated draws of Sobol sequence in scipy
- Render NumPy array in FastAPI
- Any faster and memory-efficient alternative of torch.autograd.functional.jacobian(model.decoder, latent_l)?
- Convert integer to string in Python
- starting container process caused: exec: "fastapi": executable file not found in $PATH: unknown
- How to change version of Python in PyCharm
- convert image from CV_64F to CV_8U
- Pyspark new column when otherwise results in "should be a column" error
- FastAPI as a Windows service
- Regex to match a whole number not ending in some digits
- How can I measure the width of a string rendering via tkFont without creating a window first?
- The tilde operator in Python
- 2D stacking and optimizing
- Compute percentage of positive rows in a group_by polars DataFrame
- Extract domain name from URL in Python
- How to use AssertRaisesMessage() in Django tests
- Langchain ChatOllama always produces invalid format: expected "json" error
- Python client library for Language Server Protocol (LSP)
- How can I convert a python urandom to a string?