From 4c784a7b54ed05c46275c76499e650db1f911018 Mon Sep 17 00:00:00 2001
From: jmaceiras <jeremy.maceiras@idiap.ch>
Date: Fri, 12 Aug 2022 09:43:26 +0200
Subject: [PATCH] [misc] Minor changes for release
---
cpp/src/LQT.cpp | 184 ++++++++++++++++++++---------------
data/convert_mat_to_numpy.py | 28 ------
2 files changed, 106 insertions(+), 106 deletions(-)
delete mode 100644 data/convert_mat_to_numpy.py
diff --git a/cpp/src/LQT.cpp b/cpp/src/LQT.cpp
index 5282530..fd26123 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 3208403..0000000
--- 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
--
GitLab