diff --git a/README.md b/README.md index e2feff0e9740569105e7d8dfd35fa82b84ccac6c..628fa30448d9b77fc4c5dc05a3017585e8fd7ff3 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # Robotics codes from scratch (RCFS) -RCFS is a collection of source codes to study robotics through simple 2D examples. Most examples are coded in Python and Matlab/Octave (full compatibility with GNU Octave). Some are also coded in C++ and Julia. The code examples have .m, .py, .cpp and .jl extensions that can be found in their respective folders matlab, python, cpp and julia. +Robotics codes from scratch (RCFS) is a collection of source codes to study and test learning and optimization problems in robotics through simple 2D examples. Most examples are coded in Python and Matlab/Octave (full compatibility with GNU Octave). Some are also coded in C++ and Julia. The code examples have .m, .py, .cpp and .jl extensions that can be found in their respective folders matlab, python, cpp and julia. ### List of examples diff --git a/python/IK.py b/python/IK.py index 88224b69f13dee15f151056a7a5da2fae5243582..f7055bf17b6e790c17d062508d9795a92688a871 100644 --- a/python/IK.py +++ b/python/IK.py @@ -20,7 +20,7 @@ ''' import numpy as np -import matplotlib.pyplot as fig +import matplotlib.pyplot as plt # Forward kinematics for all joints (in robot coordinate system) def fkin0(x, param): @@ -49,8 +49,8 @@ param = lambda: None # Lazy way to define an empty class in python param.dt = 1e-2 # Time step length param.nbData = 50 # Number of datapoints param.nbVarX = 3 # State space dimension (x1,x2,x3) -param.nbVarF = 2 # Objective function dimension (position of the end-effector -param.l = [2,2,1] # Robot links lengths +param.nbVarF = 3 # Objective function dimension (position and orientation of the end-effector) +param.l = [2, 2, 1] # Robot links lengths fh = np.array([-2, 1]) #Desired target for the end-effector x = np.ones(param.nbVarX) * np.pi / param.nbVarX #Initial robot pose @@ -59,13 +59,13 @@ x = np.ones(param.nbVarX) * np.pi / param.nbVarX #Initial robot pose ## Inverse kinematics (IK) # =============================== -fig.scatter(fh[0], fh[1], color='r', marker='.', s=10**2) #Plot target +plt.scatter(fh[0], fh[1], color='r', marker='.', s=10**2) #Plot target for t in range(param.nbData): f = fkin0(x, param) #Forward kinematics (for all articulations, including end-effector) J = Jkin(x, param) #Jacobian (for end-effector) x += np.linalg.pinv(J) @ (fh - f[:,-1]) * 10 * param.dt #Update state - fig.plot(f[0,:], f[1,:], color='k', linewidth=1) #Plot robot + plt.plot(f[0,:], f[1,:], color='k', linewidth=1) #Plot robot -fig.axis('off') -fig.axis('equal') -fig.show() +plt.axis('off') +plt.axis('equal') +plt.show()