diff --git a/README.md b/README.md index ac7f60f4e493a6cabfaa9af17300081dc73440c0..eff7a89d1af703995aac0a22a0ace78edeb18cba 100644 --- a/README.md +++ b/README.md @@ -15,7 +15,7 @@ The RCFS website also includes interactive exercises: [https://rcfs.ch](https:// | spline1D | Concatenated Bernstein basis functions with C0 and C1 constraints to encode a trajectory (1D input, 1D output) | ✅ | ✅ | | | | spline2D | Concatenated Bernstein basis functions with constraints to encode a signed distance function (2D inputs, 1D output) | ✅ | ✅ | | | | spline2D_eikonal | Gauss-Newton optimization of an SDF encoded with concatenated cubic polysplines, by considering unit norm derivatives in the cost function | ✅ | ✅ | | | -| IK | Inverse kinematics for a planar manipulator | ✅ | ✅ | ✅ | ✅ | +| IK_manipulator | Inverse kinematics for a planar manipulator | ✅ | ✅ | ✅ | ✅ | | IK_bimanual | Inverse kinematics with a planar bimanual robot | | ✅ | | | | IK_nullspace | Inverse kinematics with nullspace projection (position and orientation tracking as primary or secondary tasks) | ✅ | ✅ | | | | IK_num | Inverse kinematics with numerical computation for a planar manipulator | ✅ | ✅ | | | @@ -29,7 +29,7 @@ The RCFS website also includes interactive exercises: [https://rcfs.ch](https:// | LQT_recursive_LS_multiAgents | LQT applied to a multi-agent system with recursive formulation based on least squares and augmented state, by using a precision matrix with nonzero offdiagonal elements to find a controller in which the two agents coordinate their movements to find an optimal meeting point | ✅ | ✅ | | | | LQT_CP | LQT with control primitives applied to a viapoint task (batch formulation) | ✅ | ✅ | | | | LQT_CP_DMP | LQT with control primitives applied to trajectory tracking with a formulation similar to dynamical movement primitives (DMP), by using the least squares formulation of recursive LQR on an augmented state space | ✅ | ✅ | | | -| iLQR_distMaintenance | Iterative linear quadratic regulator (iLQR) applied to a 2D point-mass system with the objective of constantly maintaining a desired distance to an object | ✅ | | | | +| iLQR_distMaintenance | Iterative linear quadratic regulator (iLQR) applied to a 2D point-mass system with the objective of constantly maintaining a desired distance to an object | ✅ | ✅ | | | | iLQR_obstacle | iLQR applied to a viapoint task with obstacles avoidance (batch formulation) | ✅ | ✅ | | | | GPIS | Gaussian process implicit surfaces (GPIS) | | ✅ | | | | iLQR_obstacle_GPIS | iLQR with obstacles represented as Gaussian process implicit surfaces (GPIS) | ✅ | ✅ | ✅ | | @@ -54,7 +54,7 @@ The RCFS website also includes interactive exercises: [https://rcfs.ch](https:// RCFS is maintained by Sylvain Calinon, https://calinon.ch -Contributors: Sylvain Calinon, Philip Abbet, Jérémy Maceiras, Hakan Girgin, Julius Jankowski, Teguh Lembono, Tobias Löw, Amirreza Razmjoo, Boyang Ti, Teng Xue, Yifei Dong, Yiming Li, Cem Bilaloglu, Yan Zhang, Guillaume Clivaz +Contributors: Sylvain Calinon, Philip Abbet, Jérémy Maceiras, Hakan Girgin, Julius Jankowski, Teguh Lembono, Tobias Löw, Amirreza Razmjoo, Boyang Ti, Teng Xue, Yifei Dong, Yiming Li, Cem Bilaloglu, Yan Zhang, Guillaume Clivaz, Maximilien Dufau Copyright (c) 2024 Idiap Research Institute, https://idiap.ch diff --git a/cpp/CMakeLists.txt b/cpp/CMakeLists.txt index afbb09900dae4a3df7d61a0dbb1e350cf4ed8e87..c5723c9ad63abef770190f4790ed95d79d6361ea 100644 --- a/cpp/CMakeLists.txt +++ b/cpp/CMakeLists.txt @@ -5,11 +5,11 @@ project(robotics-codes-from-scratch) include(${CMAKE_CURRENT_LIST_DIR}/cmake/options.cmake) include(${CMAKE_CURRENT_LIST_DIR}/cmake/register_executable.cmake) -register_executable(IK) +register_executable(IK_manipulator) register_executable(LQT) register_executable(iLQR_bicopter) register_executable(iLQR_bimanual) register_executable(iLQR_car) register_executable(iLQR_manipulator) register_executable(iLQR_manipulator_obstacle) -register_executable(iLQR_obstacle_GPIS) \ No newline at end of file +register_executable(iLQR_obstacle_GPIS) diff --git a/cpp/src/IK.cpp b/cpp/src/IK_manipulator.cpp similarity index 100% rename from cpp/src/IK.cpp rename to cpp/src/IK_manipulator.cpp diff --git a/doc/rcfs.tex b/doc/rcfs.tex index 07dcaa523f2f9feea6b6fd678dedd94a28d3cc1f..2b0c11e8f4d4878a5fc1554d39c8c73047d4fdf0 100644 --- a/doc/rcfs.tex +++ b/doc/rcfs.tex @@ -411,7 +411,7 @@ minimizes the constrained cost. The first part of this augmented state then give %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% \section{Forward kinematics (FK) for a planar robot manipulator}\label{sec:FK} \begin{flushright} -\filename{IK.*} +\filename{IK\_manipulator.*} \end{flushright} %\begin{figure} @@ -503,7 +503,7 @@ f = np.array([L @ np.diag(l) @ np.cos(L @ x), L @ np.diag(l) @ np.sin(L @ x)]) # %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% \section{Inverse kinematics (IK) for a planar robot manipulator}\label{sec:IK} \begin{flushright} -\filename{IK.*} +\filename{IK\_manipulator.*} \end{flushright} By differentiating the forward kinematics function, a least norm inverse kinematics solution can be computed with diff --git a/matlab/IK.m b/matlab/IK_manipulator.m similarity index 100% rename from matlab/IK.m rename to matlab/IK_manipulator.m diff --git a/python/IK.py b/python/IK_manipulator.py similarity index 100% rename from python/IK.py rename to python/IK_manipulator.py diff --git a/python/iLQR_distMaintenance.py b/python/iLQR_distMaintenance.py new file mode 100644 index 0000000000000000000000000000000000000000..863e91b5243ccbf2b12565de985bc8a7a457af7c --- /dev/null +++ b/python/iLQR_distMaintenance.py @@ -0,0 +1,111 @@ +''' +iLQR applied to a 2D point-mass system with the objective of constantly maintaining a desired distance to an object + +Copyright (c) 2024 Idiap Research Institute <https://www.idiap.ch> +Written by Maximilien Dufau <maximilien.dufau@idiap.ch> and +Sylvain Calinon <https://calinon.ch> + +This file is part of RCFS <https://rcfs.ch> +License: MIT +''' + +import numpy as np +import matplotlib.pyplot as plt +import matplotlib.patches as patches + + +# Residual and Jacobian for an distance maintenance task +def f_dist(x,param): + e = x - np.tile(param.Mu,(1,param.nbData)) + f = np.array([1 - (np.sum(e**2,0).T/(param.dist**2))]).T + Jtmp = np.tile(- e.T/(param.dist**2),(1,param.nbData)) + J = Jtmp * np.kron(np.eye(param.nbData),np.ones((1,param.nbVarU))) + return f, J + +# General parameters +# =============================== + +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.nbIter = 100 # Maximum number of iterations for iLQR +param.nbVarX = 2 # State space dimension (x1,x2) +param.nbVarU = 2 # Control space dimension (dx1,dx2) +param.r = 1e-3 # Control weight term +param.q = 1e0 # Distance maintenance weight term + +# Task parameters +# =============================== + +# Target +param.Mu = np.array([[ 1.0, 0.3 ]]).T # Object location +param.dist = 0.4 # Distance to maintain + +# Regularization matrix +R = np.identity( (param.nbData-1) * param.nbVarU ) * param.r + +# System parameters +# =============================== + +u = np.zeros(( param.nbVarU * (param.nbData-1),1)) # Initial control command +x0 = np.zeros( (param.nbVarX,1)) # Initial state + +# Transfer matrices (for linear system as single integrator) +Su = np.vstack([np.zeros((param.nbVarX, param.nbVarX*(param.nbData-1))), + np.tril(np.kron(np.ones((param.nbData-1,param.nbData-1)), np.eye(param.nbVarX)*param.dt))]) +Sx = np.kron(np.ones((param.nbData,1)), np.eye(param.nbVarX)) + +# Solving iLQR +# =============================== + +for i in range( param.nbIter ): + x = Su @ u + Sx @ x0 + x = x.reshape( ( param.nbVarX, param.nbData ),order="F" ) + f, J = f_dist(x,param)# Avoidance objective + + du = np.linalg.inv(Su.T @ J.T @ J @ Su * param.q + R) @ (-Su.T @ J.T @ f * param.q - u * param.r) + + # Perform line search + alpha = 1 + cost0 = np.linalg.norm(f)**2 * param.q + np.linalg.norm(u)**2 * param.r + while True: + utmp = u + du * alpha + xtmp = Su @ utmp + Sx @ x0 + + xtmp = xtmp.reshape( ( param.nbVarX, param.nbData ),order="F") + ftmp,_ = f_dist(xtmp,param) + cost = np.linalg.norm(ftmp)**2 * param.q + np.linalg.norm(utmp)**2 * param.r + + if cost < cost0 or alpha < 1e-3: + u = utmp + print("Iteration {}, cost: {}, alpha: {}".format(i,cost,alpha)) + break + + alpha /=2 + + if np.linalg.norm(alpha * du) < 1e-2: # Early stop condition + break + +# Plotting +# =============================== + +plt.figure() +plt.axis("off") +ax = plt.gca() +ax.set_aspect('equal', adjustable='box') + +# Get trajectory +x = Su @ u + Sx @ x0 +x = x.reshape(( param.nbVarX, param.nbData ),order="F") + +plt.scatter(x[0,0], x[1,0], c='red', s=40) +plt.plot(x[0,:], x[1,:], c='black', alpha=0.5) +plt.scatter(x[0,:], x[1,:], c='black', s=10,alpha=0.5) + +# Plot targets +plt.scatter(param.Mu[0,0], param.Mu[1,0],marker="x", c='k', s=40) +circle = plt.Circle((param.Mu[0,0], param.Mu[1,0]), param.dist, color='k', fill = False) +ax.add_patch (circle) + + +plt.show()