diff --git a/cpp/src/LQT.cpp b/cpp/src/LQT.cpp
index 52825301cf9551727dc88f756f76f375cf05fb17..fd261234779d879f25e4ac060ab7ad278b7771c0 100644
--- a/cpp/src/LQT.cpp
+++ b/cpp/src/LQT.cpp
@@ -1,11 +1,30 @@
-// LQT.cpp
-// Sylvain Calinon, 2021
+/*
+ LQT applied to a 2D manipulator system reaching a target.
+
+ Copyright (c) 2021 Idiap Research Institute, http://www.idiap.ch/
+ Written by Sylvain Calinon <https://calinon.ch>
+
+ This file is part of RCFS.
+
+ RCFS is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License version 3 as
+ published by the Free Software Foundation.
+
+ RCFS is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with RCFS. If not, see <http://www.gnu.org/licenses/>.
+*/
-#include <iostream>
#include <Eigen/Dense>
-#include <eigen3/unsupported/Eigen/KroneckerProduct>
#include <GL/glut.h>
+#include <eigen3/unsupported/Eigen/KroneckerProduct>
+#include <iostream>
#include <math.h>
+#include <vector>
using namespace Eigen;
@@ -23,7 +42,7 @@ struct Param {
dt = 1e-2;
linkLengths = VectorXd(4);
linkLengths << 1.0, 1.0, 0.5, 0.5;
- x_d << -2, 1, 4*M_PI/4.;
+ x_d << -2, 1, 4 * M_PI / 4.;
Q_t = 0.0;
Q_T = 100.0;
R_t = 1e-2;
@@ -33,8 +52,7 @@ struct Param {
// Kinematics functions
// ===============================
-MatrixXd fkin(VectorXd q)
-{
+MatrixXd fkin(VectorXd q) {
Param param;
long int nbDoF = param.linkLengths.size();
MatrixXd G = MatrixXd::Ones(nbDoF, nbDoF).triangularView<UnitLower>();
@@ -42,61 +60,70 @@ MatrixXd fkin(VectorXd q)
x.row(0) = G * param.linkLengths.asDiagonal() * cos((G * q).array()).matrix();
x.row(1) = G * param.linkLengths.asDiagonal() * sin((G * q).array()).matrix();
x.row(2) = G * q;
-
+
return x;
}
-MatrixXd jacobian(VectorXd q)
-{
+MatrixXd jacobian(VectorXd q) {
Param param;
long int nbDoF = param.linkLengths.size();
MatrixXd G = MatrixXd::Ones(nbDoF, nbDoF).triangularView<UnitLower>();
MatrixXd J = MatrixXd::Zero(3, nbDoF);
- J.row(0) = - G.transpose() * param.linkLengths.asDiagonal() * sin((G * q).array()).matrix();
- J.row(1) = G.transpose() * param.linkLengths.asDiagonal() * cos((G * q).array()).matrix();
+ J.row(0) = -G.transpose() * param.linkLengths.asDiagonal() *
+ sin((G * q).array()).matrix();
+ J.row(1) = G.transpose() * param.linkLengths.asDiagonal() *
+ cos((G * q).array()).matrix();
J.row(2) = VectorXd::Ones(nbDoF);
-
+
return J;
}
// Iterative Inverse Kinematics
// ===============================
-std::vector<MatrixXd> IK(VectorXd& q)
-{
+std::vector<MatrixXd> IK(VectorXd &q) {
Param param;
std::vector<MatrixXd> x;
-
- for(unsigned int i = 0; i < param.nbSteps; i++) {
+
+ for (unsigned int i = 0; i < param.nbSteps; i++) {
x.push_back(fkin(q));
auto J = jacobian(q);
- auto dx_d = - (x.back().col(param.linkLengths.size()-1) - param.x_d);
- VectorXd dq_d = J.completeOrthogonalDecomposition().solve(dx_d);
- q = q + param.dt * dq_d;
+ auto dx_d = -(x.back().col(param.linkLengths.size() - 1) - param.x_d);
+ VectorXd dq_d = J.completeOrthogonalDecomposition().solve(dx_d);
+ q = q + param.dt * dq_d;
}
-
+
return x;
}
// Trajectory Optimization
// ===============================
-std::vector<VectorXd> LQT(const VectorXd& q_init, const VectorXd& q_goal, const double T=1.0)
-{
+std::vector<VectorXd> LQT(const VectorXd &q_init, const VectorXd &q_goal,
+ const double T = 1.0) {
Param param;
long int nbDoF = param.linkLengths.size();
unsigned int nbSteps = (unsigned int)(T / param.dt);
- MatrixXd Sq = kroneckerProduct(MatrixXd::Ones(nbSteps, 1), MatrixXd::Identity(nbDoF, nbDoF));
- MatrixXd Su = MatrixXd::Zero(nbDoF*nbSteps, (nbSteps-1)*nbDoF);
- Su.bottomRows((nbSteps-1)*nbDoF) =
- kroneckerProduct(MatrixXd::Ones(nbSteps-1, nbSteps-1), param.dt * MatrixXd::Identity(nbDoF, nbDoF)).eval().triangularView<Eigen::Lower>();
- VectorXd q_d = VectorXd::Zero(nbDoF*nbSteps);
+ MatrixXd Sq = kroneckerProduct(MatrixXd::Ones(nbSteps, 1),
+ MatrixXd::Identity(nbDoF, nbDoF));
+ MatrixXd Su = MatrixXd::Zero(nbDoF * nbSteps, (nbSteps - 1) * nbDoF);
+ Su.bottomRows((nbSteps - 1) * nbDoF) =
+ kroneckerProduct(MatrixXd::Ones(nbSteps - 1, nbSteps - 1),
+ param.dt * MatrixXd::Identity(nbDoF, nbDoF))
+ .eval()
+ .triangularView<Eigen::Lower>();
+ VectorXd q_d = VectorXd::Zero(nbDoF * nbSteps);
q_d.tail(nbDoF) = q_goal;
- MatrixXd Q = param.Q_t * MatrixXd::Identity(nbDoF*nbSteps, nbDoF*nbSteps);
- Q.bottomRightCorner(nbDoF, nbDoF) = param.Q_T * MatrixXd::Identity(nbDoF, nbDoF);
- MatrixXd R = param.R_t * MatrixXd::Identity(nbDoF*(nbSteps-1), nbDoF*(nbSteps-1));
- VectorXd q_opt = Sq * q_init + Su * (Su.transpose() * Q * Su + R).ldlt().solve(Su.transpose() * Q * (q_d - Sq * q_init));
+ MatrixXd Q = param.Q_t * MatrixXd::Identity(nbDoF * nbSteps, nbDoF * nbSteps);
+ Q.bottomRightCorner(nbDoF, nbDoF) =
+ param.Q_T * MatrixXd::Identity(nbDoF, nbDoF);
+ MatrixXd R = param.R_t *
+ MatrixXd::Identity(nbDoF * (nbSteps - 1), nbDoF * (nbSteps - 1));
+ VectorXd q_opt =
+ Sq * q_init + Su * (Su.transpose() * Q * Su + R)
+ .ldlt()
+ .solve(Su.transpose() * Q * (q_d - Sq * q_init));
std::vector<VectorXd> q(nbSteps);
- for(unsigned int i = 0; i < nbSteps; i++) {
- q[i] = q_opt.segment(i*nbDoF, nbDoF);
+ for (unsigned int i = 0; i < nbSteps; i++) {
+ q[i] = q_opt.segment(i * nbDoF, nbDoF);
}
return q;
}
@@ -104,69 +131,70 @@ std::vector<VectorXd> LQT(const VectorXd& q_init, const VectorXd& q_goal, const
// Plotting
// ===============================
-void plot_robot(const MatrixXd& x, const double c=0.0)
-{
+void plot_robot(const MatrixXd &x, const double c = 0.0) {
Param param;
glColor3d(c, c, c);
glLineWidth(8.0);
- glBegin(GL_LINE_STRIP);
- glVertex2d(0, 0);
- for(unsigned int j = 0; j < param.linkLengths.size(); j++){
- glVertex2d(x(0,j), x(1,j));
- }
- glEnd();
+ glBegin(GL_LINE_STRIP);
+ glVertex2d(0, 0);
+ for (unsigned int j = 0; j < param.linkLengths.size(); j++) {
+ glVertex2d(x(0, j), x(1, j));
+ }
+ glEnd();
}
-void render(){
+void render() {
Param param;
- VectorXd q_init = VectorXd::Ones(param.linkLengths.size()) * M_PI / (double)param.linkLengths.size();
+ VectorXd q_init = VectorXd::Ones(param.linkLengths.size()) * M_PI /
+ (double)param.linkLengths.size();
VectorXd q_goal = q_init;
auto x_IK = IK(q_goal);
-
+
auto q = LQT(q_init, q_goal, param.T);
-
+
std::vector<MatrixXd> x;
- for(auto& q_ : q) {
+ for (auto &q_ : q) {
x.push_back(fkin(q_));
}
- glLoadIdentity();
- double d = (double)param.linkLengths.size() * .7;
- glOrtho(-d, d, -.1, d-.1, -1.0, 1.0);
- glClearColor(1.0, 1.0, 1.0, 1.0);
- glClear(GL_COLOR_BUFFER_BIT);
+ glLoadIdentity();
+ double d = (double)param.linkLengths.size() * .7;
+ glOrtho(-d, d, -.1, d - .1, -1.0, 1.0);
+ glClearColor(1.0, 1.0, 1.0, 1.0);
+ glClear(GL_COLOR_BUFFER_BIT);
// Plot start and end configuration
plot_robot(x.front(), 0.8);
plot_robot(x.back(), 0.4);
-
- // Plot end-effector trajectory
- double c = 0.0;
+
+ // Plot end-effector trajectory
+ double c = 0.0;
glColor3d(c, c, c);
glLineWidth(4.0);
- glBegin(GL_LINE_STRIP);
- for(auto& x_ : x) {
- glVertex2d(x_(0,param.linkLengths.size()-1), x_(1,param.linkLengths.size()-1));
- }
- glEnd();
-
- // Plot target
- glColor3d(1.0, 0, 0);
- glPointSize(12.0);
- glBegin(GL_POINTS);
- glVertex2d(param.x_d(0), param.x_d(1));
- glEnd();
-
- //Render
- glutSwapBuffers();
+ glBegin(GL_LINE_STRIP);
+ for (auto &x_ : x) {
+ glVertex2d(x_(0, param.linkLengths.size() - 1),
+ x_(1, param.linkLengths.size() - 1));
+ }
+ glEnd();
+
+ // Plot target
+ glColor3d(1.0, 0, 0);
+ glPointSize(12.0);
+ glBegin(GL_POINTS);
+ glVertex2d(param.x_d(0), param.x_d(1));
+ glEnd();
+
+ // Render
+ glutSwapBuffers();
}
-int main(int argc, char** argv){
- glutInit(&argc, argv);
- glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
- glutInitWindowPosition(20,20);
- glutInitWindowSize(1200,600);
- glutCreateWindow("IK");
- glutDisplayFunc(render);
- glutMainLoop();
+int main(int argc, char **argv) {
+ glutInit(&argc, argv);
+ glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
+ glutInitWindowPosition(20, 20);
+ glutInitWindowSize(1200, 600);
+ glutCreateWindow("IK");
+ glutDisplayFunc(render);
+ glutMainLoop();
}
diff --git a/data/convert_mat_to_numpy.py b/data/convert_mat_to_numpy.py
deleted file mode 100644
index 3208403291a6467bfe9cc4888c5b81bd170626d9..0000000000000000000000000000000000000000
--- a/data/convert_mat_to_numpy.py
+++ /dev/null
@@ -1,28 +0,0 @@
-from pathlib import Path
-import numpy as np
-import matplotlib.pyplot as plt
-import scipy.io
-import os
-
-FILE_PATH = os.path.dirname(os.path.realpath(__file__))
-DATASET_LETTERS_FOLDER = "2Dletters/"
-
-dataset_folder = Path(FILE_PATH,DATASET_LETTERS_FOLDER)
-file_list = [str(p) for p in dataset_folder.rglob('*') if p.is_file() and p.match('*.mat')]
-
-for file_name in file_list:
- junk_array = scipy.io.loadmat(file_name)["demos"].flatten()
-
- letter_demos = []
-
- for demo in junk_array:
- pos = demo[0]['pos'].flatten()[0].T
- vel = demo[0]['vel'].flatten()[0].T
- acc = demo[0]['acc'].flatten()[0].T
-
- demo_concatenated = np.hstack(( pos , vel , acc ))
- letter_demos += [demo_concatenated]
-
- letter_demos = np.asarray(letter_demos)
- file_name_npy = Path( Path(file_name).parent , Path(file_name).stem + ".npy")
- np.save(file_name_npy,letter_demos)
\ No newline at end of file
diff --git a/matlab/iLQR_manipulator_CoM.m b/matlab/iLQR_manipulator_CoM.m
index 31fa7701e7883ff32e119fcf7605cebf24ca0b2e..ce657f36ed714d8394baef8ea4f89fcb2bc363c3 100644
--- a/matlab/iLQR_manipulator_CoM.m
+++ b/matlab/iLQR_manipulator_CoM.m
@@ -162,7 +162,7 @@ function [f, J] = f_reach_CoM(x, param)
cos(L * x(:,t))' * L * diag(param.l' * L)] / param.nbVarX;
%Bounding boxes (optional)
- for i=1:1
+ for i=1:1 %Only x direction
if abs(f(i,t)) < param.szCoM
f(i,t) = 0;
Jtmp(i,:) = 0;
diff --git a/python/IK.py b/python/IK.py
index b96f453f44af4566852f31290b77edd6364bcfd5..88224b69f13dee15f151056a7a5da2fae5243582 100644
--- a/python/IK.py
+++ b/python/IK.py
@@ -22,19 +22,48 @@
import numpy as np
import matplotlib.pyplot as fig
-T = 50 #Number of datapoints
-D = 3 #State space dimension (x1,x2,x3)
-l = np.array([2, 2, 1]); #Robot links lengths
+# Forward kinematics for all joints (in robot coordinate system)
+def fkin0(x, param):
+ L = np.tril(np.ones([param.nbVarX, param.nbVarX]))
+ f = np.vstack([
+ L @ np.diag(param.l) @ np.cos(L @ x),
+ L @ np.diag(param.l) @ np.sin(L @ x)
+ ])
+ f = np.hstack([np.zeros([2,1]), f])
+ return f
+
+# Jacobian with analytical computation (for single time step)
+def Jkin(x, param):
+ L = np.tril(np.ones([param.nbVarX, param.nbVarX]))
+ J = np.vstack([
+ -np.sin(L @ x).T @ np.diag(param.l) @ L,
+ np.cos(L @ x).T @ np.diag(param.l) @ L
+ ])
+ return J
+
+
+## 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.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
+
fh = np.array([-2, 1]) #Desired target for the end-effector
-x = np.ones(D) * np.pi / D #Initial robot pose
-L = np.tril(np.ones([D,D])) #Transformation matrix
+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
-for t in range(T):
- f = np.array([L @ np.diag(l) @ np.cos(L @ x), L @ np.diag(l) @ np.sin(L @ x)]) #Forward kinematics (for all articulations, including end-effector)
- J = np.array([-np.sin(L @ x).T @ np.diag(l) @ L, np.cos(L @ x).T @ np.diag(l) @ L]) #Jacobian (for end-effector)
- x += np.linalg.pinv(J) @ (fh - f[:,-1]) * .1 #Update state
- f = np.concatenate((np.zeros([2,1]), f), axis=1) #Add robot base (for plotting)
+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
fig.axis('off')
diff --git a/python/IK_num.py b/python/IK_num.py
index 1563d58a6d223cce7c684570f17c10a1a493721a..b2b15015848b3952ef8ca29681877b6c85450ac9 100644
--- a/python/IK_num.py
+++ b/python/IK_num.py
@@ -2,7 +2,7 @@
Inverse kinematics with numerical computation for a planar manipulator
Copyright (c) 2021 Idiap Research Institute, http://www.idiap.ch/
- Written by Jérémy Maceiras <jeremy.maceiras@idiap.ch>,Sylvain Calinon <https://calinon.ch>
+ Written by Jérémy Maceiras <jeremy.maceiras@idiap.ch>, Sylvain Calinon <https://calinon.ch>
This file is part of RCFS.
@@ -24,27 +24,26 @@ import matplotlib.pyplot as fig
import copy
# Forward kinematics (for end-effector)
-def fkin(x,l):
- L = np.tril(np.ones([len(x),len(x)])) #Transformation matrix
+def fkin(x, l):
+ L = np.tril(np.ones([len(x),len(x)])) # Transformation matrix
f = np.array([l.T @ np.cos(L @ x),l.T @ np.sin(L@x)])
return f
# Forward kinematics (for all articulation)
-def fkin0(x,l):
- L = np.tril(np.ones([len(x),len(x)])) #Transformation matrix
- f = np.array([L @ np.diag(l) @ np.cos(L @ x), L @ np.diag(l) @ np.sin(L @ x)]) #Forward kinematics (for all articulations, including end-effector)
-
+def fkin0(x, l):
+ L = np.tril(np.ones([len(x),len(x)])) # Transformation matrix
+ f = np.array([L @ np.diag(l) @ np.cos(L @ x), L @ np.diag(l) @ np.sin(L @ x)]) # Forward kinematics (for all articulations, including end-effector)
return f
# Jacobian with numerical computation
-def jkin(x,l):
+def Jkin(x, l):
eps = 1e-6
D = len(x)
# Matrix computation
- X = np.tile(x.reshape((-1,1)),[1,D])
- F1 = fkin(X,l)
- F2 = fkin(X+np.identity(D)*eps,l)
+ X = np.tile(x.reshape((-1,1)), [1,D])
+ F1 = fkin(X, l)
+ F2 = fkin(X+np.identity(D)*eps, l)
J = (F2-F1) / eps
# # For loop computation
@@ -67,8 +66,8 @@ x = np.ones(D) * np.pi / D #Initial robot pose
fig.scatter(fh[0], fh[1], color='r', marker='.', s=10**2) #Plot target
for t in range(T):
- J = jkin(x,l)
- f = fkin0(x,l)
+ J = Jkin(x, l)
+ f = fkin0(x, l)
x += np.linalg.pinv(J) @ (fh - f[:,-1]) * .1 #Update state
f = np.concatenate((np.zeros([2,1]), f), axis=1) #Add robot base (for plotting)
diff --git a/python/iLQR_manipulator.py b/python/iLQR_manipulator.py
index 2aa963d47157c32745eb03efae5692997e361aa9..1b10ac3f182a0b6685f55b29489a444a0b0f71ee 100644
--- a/python/iLQR_manipulator.py
+++ b/python/iLQR_manipulator.py
@@ -82,7 +82,7 @@ def f_reach(x, param):
f[i,t] -= np.sign(f[i,t]) * param.sz[i]
J[t*param.nbVarF:(t+1)*param.nbVarF, t*param.nbVarX:(t+1)*param.nbVarX] = Jtmp
- return f,J
+ return f, J
## Parameters
# ===============================
@@ -98,8 +98,8 @@ param.nbVarF = 3 # Objective function dimension (f1,f2,f3, with f3 as orientatio
param.l = [2,2,1] # Robot links lengths
param.sz = [.2,.3] # Size of objects
param.r = 1e-6 # Control weight term
-param.Mu = np.asarray([[2,1,-np.pi/6], [3,2,-np.pi/3]]).T # Viapoints
-param.A = np.zeros([2,2,param.nbPoints]) # Object orientation matrices
+param.Mu = np.asarray([[2, 1, -np.pi/6], [3, 2, -np.pi/3]]).T # Viapoints
+param.A = np.zeros([2, 2,param.nbPoints]) # Object orientation matrices
param.useBoundingBox = True # Consider bounding boxes for reaching cost
@@ -143,7 +143,7 @@ x0 = np.array([3*np.pi/4, -np.pi/2, -np.pi/4]) # Initial state
for i in range(param.nbIter):
x = Su0 @ u + Sx0 @ x0 # System evolution
x = x.reshape([param.nbVarX, param.nbData], order='F')
- f,J = f_reach(x[:,tl], param) # Residuals and Jacobians
+ f, J = f_reach(x[:,tl], param) # Residuals and Jacobians
du = np.linalg.inv(Su.T @ J.T @ Q @ J @ Su + R) @ (-Su.T @ J.T @ Q @ f.flatten('F') - u * param.r) # Gauss-Newton update
# Estimate step size with backtracking line search method
alpha = 1
diff --git a/python/iLQR_manipulator_CP.py b/python/iLQR_manipulator_CP.py
index f92a7dd3c1ae47d1989b608ce8111d705e68d100..bbd43d3c9a630ae6e547fd1157d9feb86686af89 100644
--- a/python/iLQR_manipulator_CP.py
+++ b/python/iLQR_manipulator_CP.py
@@ -166,7 +166,7 @@ Q = np.identity( param.nbVarF * param.nbPoints)
# System parameters
# ===============================
-# Time occurence of viapoints
+# Time occurrence of viapoints
tl = np.linspace(0,param.nbData,param.nbPoints+1)
tl = np.rint(tl[1:]).astype(np.int64)-1
idx = np.array([ i + np.arange(0,param.nbVarX,1) for i in (tl* param.nbVarX)])
@@ -223,7 +223,7 @@ for i in range( param.nbIter ):
if np.linalg.norm( alpha * du ) < 1e-2:
break
-# Ploting
+# Plotting
# ===============================
plt.figure()
@@ -235,7 +235,7 @@ f = fkin(param,x)
f00 = fkin0(param,x[0])
f10 = fkin0(param,x[tl[0]])
fT0 = fkin0(param,x[-1])
-u = u .reshape((-1,param.nbVarU))
+u = u.reshape((-1,param.nbVarU))
plt.plot( f00[:,0] , f00[:,1],c='black',linewidth=5,alpha=.2)
plt.plot( f10[:,0] , f10[:,1],c='black',linewidth=5,alpha=.4)
diff --git a/python/iLQR_manipulator_CoM.py b/python/iLQR_manipulator_CoM.py
index 6f74266847d4381d6aab25d8b64af172a8741664..4ab27da97e8c5b44ed8b50d8dfa74ae5a794709d 100644
--- a/python/iLQR_manipulator_CoM.py
+++ b/python/iLQR_manipulator_CoM.py
@@ -28,72 +28,66 @@ import matplotlib.patches as patches
# Helper functions
# ===============================
-# Forward kinematics (in robot coordinate system)
-def fkin(param,x):
- x = x.T
- A = np.tril(np.ones([param.nbVarX, param.nbVarX]))
- f = np.vstack((param.l @ np.cos(A @ x),
- param.l @ np.sin(A @ x)))
- return f
-# Forward Kinematics for all joints
-def fkin0(param,x):
- T = np.tril(np.ones([param.nbVarX, param.nbVarX]))
- T2 = np.tril(np.matlib.repmat(param.l, len(x), 1))
- f = np.vstack((
- T2 @ np.cos(T @ x),
- T2 @ np.sin(T @ x)
- )).T
- f = np.vstack((
- np.zeros(2),
- f
- ))
- return f
+# Forward kinematics for end-effector (in robot coordinate system)
+def fkin(x, param):
+ L = np.tril(np.ones([param.nbVarX, param.nbVarX]))
+ f = np.vstack([
+ param.l @ np.cos(L @ x),
+ param.l @ np.sin(L @ x)
+ ])
+ return f
+
+# Forward kinematics for all joints (in robot coordinate system)
+def fkin0(x, param):
+ L = np.tril(np.ones([param.nbVarX, param.nbVarX]))
+ f = np.vstack([
+ L @ np.diag(param.l) @ np.cos(L @ x),
+ L @ np.diag(param.l) @ np.sin(L @ x)
+ ])
+ f = np.hstack([np.zeros([2,1]), f])
+ return f
# Jacobian with analytical computation (for single time step)
-def jkin(param,x):
- T = np.tril(np.ones((len(x), len(x))))
- J = np.vstack((
- -np.sin(T @ x).T @ np.diag(param.l) @ T,
- np.cos(T @ x).T @ np.diag(param.l) @ T
- ))
- return J
-
-# Residual and Jacobian
-def f_reach(param,x):
- f = fkin(param,x).T - param.Mu
- J = np.zeros((len(x) * param.nbVarF, len(x) * param.nbVarX))
-
- for t in range(x.shape[0]):
- Jtmp = jkin(param,x[t])
- J[t * param.nbVarF:(t + 1) * param.nbVarF, t * param.nbVarX:(t + 1) * param.nbVarX] = Jtmp
- return f.T, J
+def Jkin(x, param):
+ L = np.tril(np.ones([param.nbVarX, param.nbVarX]))
+ J = np.vstack([
+ -np.sin(L @ x).T @ np.diag(param.l) @ L,
+ np.cos(L @ x).T @ np.diag(param.l) @ L
+ ])
+ return J
+
+# Residual and Jacobian for a viapoints reaching task (in object coordinate system)
+def f_reach(x, param):
+ f = fkin(x, param) - param.Mu.reshape((-1,1))
+ J = np.zeros([param.nbPoints * param.nbVarF, param.nbPoints * param.nbVarX])
+ for t in range(param.nbPoints):
+ J[t*param.nbVarF:(t+1)*param.nbVarF, t*param.nbVarX:(t+1)*param.nbVarX] = Jkin(x[:,t], param)
+ return f, J
# Forward kinematics for center of mass (in robot coordinate system, with mass located at the joints)
-def fkin_CoM(param,x):
- x = x.T
- A = np.tril(np.ones([param.nbVarX, param.nbVarX]))
- f = np.vstack((param.l @ A @ np.cos(A @ x),
- param.l @ A @ np.sin(A @ x))) / param.nbVarX
+def fkin_CoM(x, param):
+ L = np.tril(np.ones([param.nbVarX, param.nbVarX]))
+ f = np.vstack((param.l @ L @ np.cos(L @ x),
+ param.l @ L @ np.sin(L @ x))) / param.nbVarX
return f
# Residual and Jacobian for center of mass
-def f_reach_CoM(param,x):
- f = fkin_CoM(param,x).T - param.MuCoM
- J = np.zeros((len(x) * param.nbVarF, len(x) * param.nbVarX))
- A = np.tril(np.ones([param.nbVarX, param.nbVarX]))
- for t in range(x.shape[0]):
- Jtmp = np.vstack((-np.sin(A @ x[t] ).T @ A @ np.diag(param.l @ A) ,
- np.cos(A @ x[t] ).T @ A @ np.diag(param.l @ A)))/param.nbVarX
+def f_reach_CoM(x, param):
+ f = fkin_CoM(x, param) - np.tile(param.MuCoM.reshape((-1,1)), [1,np.size(x,1)])
+ J = np.zeros((np.size(x,1) * param.nbVarF, np.size(x,1) * param.nbVarX))
+ L = np.tril(np.ones([param.nbVarX, param.nbVarX]))
+ for t in range(np.size(x,1)):
+ Jtmp = np.vstack((-np.sin(L @ x[:,t] ).T @ L @ np.diag(param.l @ L) ,
+ np.cos(L @ x[:,t] ).T @ L @ np.diag(param.l @ L))) / param.nbVarX
if param.useBoundingBox:
- for i in range(1):
- if abs(f[t, i]) < param.szCoM:
- f[t, i] = 0
+ for i in range(1): # Only x direction matters
+ if abs(f[i,t]) < param.szCoM:
+ f[i,t] = 0
Jtmp[i] = 0
else:
- f[t, i] -= np.sign(f[t, i]) * param.szCoM
- J[t * param.nbVarF:(t + 1) * param.nbVarF, t * param.nbVarX:(t + 1) * param.nbVarX] = Jtmp
- f = f.flatten().T
+ f[i,t] -= np.sign(f[i,t]) * param.szCoM
+ J[t*param.nbVarF:(t+1)*param.nbVarF, t*param.nbVarX:(t+1)*param.nbVarX] = Jtmp
return f, J
@@ -113,6 +107,7 @@ param.r = 1e-5 # Control weight term
param.Mu = np.asarray([3.5, 4]) # Target
param.MuCoM = np.asarray([.4, 0]) # desired position of the center of mass
+
# Main program
# ===============================
@@ -129,10 +124,10 @@ Qc = np.kron(np.identity(param.nbData), np.diag([1E0, 0]))
# System parameters
# ===============================
-# Time occurence of viapoints
+# Time occurrence of viapoints
tl = np.linspace(0, param.nbData, param.nbPoints + 1)
tl = np.rint(tl[1:]).astype(np.int64) - 1
-idx = np.array([ i + np.arange(0,param.nbVarX,1) for i in (tl* param.nbVarX)])
+idx = np.array([i + np.arange(0,param.nbVarX,1) for i in (tl* param.nbVarX)])
u = np.zeros(param.nbVarU * (param.nbData - 1)) # Initial control command
a = .7
@@ -141,31 +136,32 @@ x0 = np.array([np.pi / 2 - a, 2 * a, - a, np.pi - np.pi / 4, 3 * np.pi / 4]) #
# Transfer matrices (for linear system as single integrator)
Su0 = 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))])
+ np.eye(param.nbVarX) * param.dt))])
Sx0 = np.kron(np.ones(param.nbData), np.identity(param.nbVarX)).T
Su = Su0[idx.flatten()] # We remove the lines that are out of interest
+
# Solving iLQR
# ===============================
for i in range(param.nbIter):
x = Su0 @ u + Sx0 @ x0
- x = x.reshape((param.nbData, param.nbVarX))
-
- f, J = f_reach(param,x[tl])
- fc, Jc = f_reach_CoM(param,x)
- du = np.linalg.inv(Su.T @ J.T @ Q @ J @ Su + Su0.T @ Jc.T @ Qc @ Jc @ Su0 + R) @ (-Su.T @ J.T @ Q @ f.flatten() - Su0.T @ Jc.T @ Qc @ fc.flatten() - u * param.r)
+ x = x.reshape([param.nbVarX, param.nbData], order='F')
+ f, J = f_reach(x[:,tl], param)
+ fc, Jc = f_reach_CoM(x, param)
+ du = np.linalg.inv(Su.T @ J.T @ Q @ J @ Su + Su0.T @ Jc.T @ Qc @ Jc @ Su0 + R) @ \
+ (-Su.T @ J.T @ Q @ f.flatten('F') - Su0.T @ Jc.T @ Qc @ fc.flatten('F') - u * param.r)
# Perform line search
alpha = 1
- cost0 = f.flatten() @ Q @ f.flatten() + fc.flatten() @ Qc @ fc.flatten() + np.linalg.norm(u) * param.r
+ cost0 = f.flatten('F').T @ Q @ f.flatten('F') + fc.flatten('F').T @ Qc @ fc.flatten('F') + np.linalg.norm(u) * param.r
while True:
utmp = u + du * alpha
xtmp = Su0 @ utmp + Sx0 @ x0
- xtmp = xtmp.reshape((param.nbData, param.nbVarX))
- ftmp, _ = f_reach(param,xtmp[tl])
- fctmp, _ = f_reach_CoM(param,xtmp)
- cost = ftmp.flatten() @ Q @ ftmp.flatten() + fctmp. T @ Qc @ fctmp + np.linalg.norm(utmp) * param.r
+ xtmp = xtmp.reshape([param.nbVarX, param.nbData], order='F')
+ ftmp, _ = f_reach(xtmp[:,tl], param)
+ fctmp, _ = f_reach_CoM(xtmp, param)
+ cost = ftmp.flatten('F').T @ Q @ ftmp.flatten('F') + fctmp.flatten('F').T @ Qc @ fctmp.flatten('F') + np.linalg.norm(utmp)**2 * param.r
if cost < cost0 or alpha < 1e-3:
u = utmp
@@ -174,7 +170,7 @@ for i in range(param.nbIter):
alpha /= 2
-# Ploting
+# Plotting
# ===============================
plt.figure()
plt.axis("off")
@@ -184,12 +180,12 @@ plt.gca().set_aspect('equal', adjustable='box')
plt.plot([-1, 3], [0, 0], linestyle='-', c=[.2, .2, .2], linewidth=2)
# Get points of interest
-f00 = fkin0(param,x[0])
-fT0 = fkin0(param,x[-1])
-fc = fkin_CoM(param,x)
+f00 = fkin0(x[:,0], param)
+fT0 = fkin0(x[:,-1], param)
+fc = fkin_CoM(x, param)
-plt.plot(f00[:, 0], f00[:, 1], c=[.8, .8, .8], linewidth=4, linestyle='-')
-plt.plot(fT0[:, 0], fT0[:, 1], c=[.4, .4, .4], linewidth=4, linestyle='-')
+plt.plot(f00[0,:], f00[1,:], c=[.8, .8, .8], linewidth=4, linestyle='-')
+plt.plot(fT0[0,:], fT0[1,:], c=[.4, .4, .4], linewidth=4, linestyle='-')
#plot CoM
plt.plot(fc[0, 0], fc[1, 0], c=[.5, .5, .5], marker="o", markeredgewidth=4, markersize=8, markerfacecolor='white')
diff --git a/python/iLQR_obstacle.py b/python/iLQR_obstacle.py
index 6de5c063bee4a32e215dc95e64bba4a1bf7906ed..10ba23c33ee40ea7207640d626470c82df19c6a4 100644
--- a/python/iLQR_obstacle.py
+++ b/python/iLQR_obstacle.py
@@ -28,7 +28,7 @@ import matplotlib.patches as patches
def f_reach(x,param):
f = x - param.Mu[:,:2]
J = np.identity(x.shape[1]*x.shape[0])
- return f,J
+ return f, J
# Residual and Jacobian for an obstacle avoidance task
def f_avoid(x,param):
@@ -56,7 +56,7 @@ def f_avoid(x,param):
f = np.array(f)
idx = np.array(idx)
idt = np.array(idt)
- return f,J.T,idx,idt
+ return f, J.T, idx, idt
# General parameters
# ===============================