From b5a909490aef1e71a0ed8170eafafa72a2e3496a Mon Sep 17 00:00:00 2001
From: Sylvain CALINON <sylvain.calinon@idiap.ch>
Date: Wed, 13 Mar 2024 12:38:05 +0100
Subject: [PATCH] IK example renamed
---
README.md | 6 +-
cpp/CMakeLists.txt | 4 +-
cpp/src/{IK.cpp => IK_manipulator.cpp} | 0
doc/rcfs.tex | 4 +-
matlab/{IK.m => IK_manipulator.m} | 0
python/{IK.py => IK_manipulator.py} | 0
python/iLQR_distMaintenance.py | 111 +++++++++++++++++++++++++
7 files changed, 118 insertions(+), 7 deletions(-)
rename cpp/src/{IK.cpp => IK_manipulator.cpp} (100%)
rename matlab/{IK.m => IK_manipulator.m} (100%)
rename python/{IK.py => IK_manipulator.py} (100%)
create mode 100644 python/iLQR_distMaintenance.py
diff --git a/README.md b/README.md
index ac7f60f..eff7a89 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 afbb099..c5723c9 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 07dcaa5..2b0c11e 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 0000000..863e91b
--- /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()
--
GitLab