In [1]:
import modern_robotics as mr
import inspect
import numpy as np
Get IKinBody function definition for reference
In [2]:
print(inspect.getsource(mr.IKinBody))
def IKinBody(Blist, M, T, thetalist0, eomg, ev):
"""Computes inverse kinematics in the body frame for an open chain robot
:param Blist: The joint screw axes in the end-effector frame when the
manipulator is at the home position, in the format of a
matrix with axes as the columns
:param M: The home configuration of the end-effector
:param T: The desired end-effector configuration Tsd
:param thetalist0: An initial guess of joint angles that are close to
satisfying Tsd
:param eomg: A small positive tolerance on the end-effector orientation
error. The returned joint angles must give an end-effector
orientation error less than eomg
:param ev: A small positive tolerance on the end-effector linear position
error. The returned joint angles must give an end-effector
position error less than ev
:return thetalist: Joint angles that achieve T within the specified
tolerances,
:return success: A logical value where TRUE means that the function found
a solution and FALSE means that it ran through the set
number of maximum iterations without finding a solution
within the tolerances eomg and ev.
Uses an iterative Newton-Raphson root-finding method.
The maximum number of iterations before the algorithm is terminated has
been hardcoded in as a variable called maxiterations. It is set to 20 at
the start of the function, but can be changed if needed.
Example Input:
Blist = np.array([[0, 0, -1, 2, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 1, 0, 0, 0.1]]).T
M = np.array([[-1, 0, 0, 0],
[ 0, 1, 0, 6],
[ 0, 0, -1, 2],
[ 0, 0, 0, 1]])
T = np.array([[0, 1, 0, -5],
[1, 0, 0, 4],
[0, 0, -1, 1.6858],
[0, 0, 0, 1]])
thetalist0 = np.array([1.5, 2.5, 3])
eomg = 0.01
ev = 0.001
Output:
(np.array([1.57073819, 2.999667, 3.14153913]), True)
"""
thetalist = np.array(thetalist0).copy()
i = 0
maxiterations = 20
Vb = se3ToVec(MatrixLog6(np.dot(TransInv(FKinBody(M, Blist, \
thetalist)), T)))
err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg \
or np.linalg.norm([Vb[3], Vb[4], Vb[5]]) > ev
while err and i < maxiterations:
thetalist = thetalist \
+ np.dot(np.linalg.pinv(JacobianBody(Blist, \
thetalist)), Vb)
i = i + 1
Vb \
= se3ToVec(MatrixLog6(np.dot(TransInv(FKinBody(M, Blist, \
thetalist)), T)))
err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg \
or np.linalg.norm([Vb[3], Vb[4], Vb[5]]) > ev
return (thetalist, not err)
Following Example 4.5 (Figure 4.6) in the book now:
Link constants:
In [3]:
L_1 = 0.425
L_2 = 0.392
H_1 = 0.089
H_2 = 0.095
W_1 = 0.109
W_2 = 0.082
L_1,L_2,H_1,H_2,W_1,W_2
Out[3]:
(0.425, 0.392, 0.089, 0.095, 0.109, 0.082)
Home position of end effector:
In [4]:
M = np.array([[-1, 0, 0, L_1+L_2],[0,0,1,W_1+W_2],[0,1,0,H_1-H_2],[0,0,0,1]])
M
Out[4]:
array([[-1. , 0. , 0. , 0.817],
[ 0. , 0. , 1. , 0.191],
[ 0. , 1. , 0. , -0.006],
[ 0. , 0. , 0. , 1. ]])
Screw axes, in base frame (end effector frame):
In [5]:
B = np.array([[0,1,0,W_1+W_2,0,L_1+L_2],[0,0,1,H_2,-L_1-L_2,0],[0,0,1,H_2,-L_2,0],[0,0,1,H_2,0,0],[0,-1,0,-W_2,0,0],[0,0,1,0,0,0]]).T
B
Out[5]:
array([[ 0. , 0. , 0. , 0. , 0. , 0. ],
[ 1. , 0. , 0. , 0. , -1. , 0. ],
[ 0. , 1. , 1. , 1. , 0. , 1. ],
[ 0.191, 0.095, 0.095, 0.095, -0.082, 0. ],
[ 0. , -0.817, -0.392, 0. , 0. , 0. ],
[ 0.817, 0. , 0. , 0. , 0. , 0. ]])
Desired end-effector configuration:
In [6]:
T_sd = np.array([[0,1,0,-0.5],[0,0,-1,0.1],[-1,0,0,0.1],[0,0,0,1]])
T_sd
Out[6]:
array([[ 0. , 1. , 0. , -0.5],
[ 0. , 0. , -1. , 0.1],
[-1. , 0. , 0. , 0.1],
[ 0. , 0. , 0. , 1. ]])
Suggested error thresholds:
In [7]:
e_omg = 0.001
e_v = 0.0001
Now, start with an initial joint coordinate guess (derived while playing around in CoppeliaSim):
In [8]:
theta_0 = np.array([2.614, 0.631, -1.472, -1.833, -5.739, 1.052])
First, I try the given function to test ability for my current guess to find the desired solution:
In [9]:
theta_d = mr.IKinBody(B, M, T_sd, theta_0, e_omg, e_v)
theta_d
Out[9]:
(array([ 2.58620026, 0.98190847, -1.7396309 , -2.38389928, -5.72780711,
1.5708314 ]),
True)
Exact solution found. Plugged into CoppeliaSim for verification:
In [15]:
from IPython.display import Image, display
display(Image(filename='./IKinBody_sim_result.png'))
Now we'll define our own function, IKinBodyIterates to log the iterations save each joint coordinate guess:
In [11]:
import logging
import csv
def IKinBodyIterates(Blist, M, T, thetalist0, eomg, ev):
"""Computes inverse kinematics in the body frame for an open chain robot
Logic is identical to modern_robotics.IKinBody but does two extra things:
1. Writes DEBUG to a logfile
2. Writes each theta guess to "iterates.csv" (no header)
"""
logging.basicConfig(
filename="log.txt",
filemode="w",
level=logging.DEBUG,
format="%(message)s"
)
file = open("iterates.csv", mode="w", newline="")
writer = csv.writer(file)
def log(T_theta, e_V_b, e_omg_mag, e_v_mag):
debug_st = f"""
Iteration: {i}
joint vector (rad): {thetalist}
SE(3) end-effector config: {T_theta}
error twist V_b: {e_V_b}
angular error magnitude ||omega_b||: {e_omg_mag}
linear error magnitude ||v_b||: {e_v_mag}
"""
logging.debug(debug_st)
writer.writerow(thetalist)
file.flush()
thetalist = np.array(thetalist0).copy()
i = 0
maxiterations = 20
def get_config_twist_and_errors():
T_theta = mr.FKinBody(M, Blist, thetalist)
Vb = mr.se3ToVec(mr.MatrixLog6(np.dot(mr.TransInv(T_theta), T)))
e_omg_mag = np.linalg.norm([Vb[0], Vb[1], Vb[2]])
e_v_mag = np.linalg.norm([Vb[3], Vb[4], Vb[5]])
return (T_theta, Vb, e_omg_mag, e_v_mag)
T_theta, e_V_b, e_omg_mag, e_v_mag = get_config_twist_and_errors()
err = (e_omg_mag > eomg) or (e_v_mag > ev)
log(T_theta, e_V_b, e_omg_mag, e_v_mag)
while err and i < maxiterations:
thetalist = thetalist \
+ np.dot(np.linalg.pinv(mr.JacobianBody(Blist, \
thetalist)), e_V_b)
i = i + 1
T_theta, e_V_b, e_omg_mag, e_v_mag = get_config_twist_and_errors()
err = (e_omg_mag > eomg) or (e_v_mag > ev)
log(T_theta, e_V_b, e_omg_mag, e_v_mag)
file.close()
return (thetalist, not err)
In [12]:
theta_d_new = IKinBodyIterates(B, M, T_sd, theta_0, e_omg, e_v)
theta_d_new
Out[12]:
(array([ 2.58620026, 0.98190847, -1.7396309 , -2.38389928, -5.72780711,
1.5708314 ]),
True)
Just to confirm reported thetas are the same as original method's:
In [14]:
np.all(theta_d_new[0] == theta_d[0])
Out[14]:
np.True_
And a video of the different joint configurations tested while solving the IK problem:
In [18]:
from IPython.display import YouTubeVideo
# Replace with the YouTube video ID (the part after `v=` in the URL)
YouTubeVideo("A9VHL7T3znQ", width=640, height=360)
Out[18]:
In [ ]: