diff --git a/README.md b/README.md
index e21191dc97ca284fc6c14d4e794e9718abd8f0ce..aaa7ccc2728ee1f138d797c7c507f688ca7e3b60 100644
--- a/README.md
+++ b/README.md
@@ -19,17 +19,17 @@ RCFS is a collection of source codes to study robotics through simple 2D example
| LQT_recursive_LS | LQT applied to a viapoint task with a recursive formulation based on least squares and an augmented state space to find a controller | ✅ | ✅ | | |
| 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 | ✅ | | | |
+| 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_obstacle | Iterative linear quadratic regulator (iLQR) applied to a viapoint task with obstacles avoidance (batch formulation) | ✅ | ✅ | | |
| iLQR_obstacle_GPIS | iLQR with obstacles represented as Gaussian process implicit surfaces (GPIS) | ✅ | | | |
| iLQR_manipulator | iLQR applied to a planar manipulator for a viapoints task (batch formulation) | ✅ | ✅ | ✅ | |
| iLQR_manipulator_recursive | iLQR applied to a planar manipulator for a viapoints task (recursive formulation to find a controller) | ✅ | ✅ | | |
-| iLQR_manipulator_CoM | iLQR applied to a planar manipulator for a tracking problem involving the center of mass (CoM) and the end-effector (batch formulation) | ✅ | | | |
-| iLQR_manipulator_obstacle | iLQR applied to a planar manipulator for a viapoints task with obstacles avoidance (batch formulation) | ✅ | | | |
+| iLQR_manipulator_CoM | iLQR applied to a planar manipulator for a tracking problem involving the center of mass (CoM) and the end-effector (batch formulation) | ✅ | ✅ | | |
+| iLQR_manipulator_obstacle | iLQR applied to a planar manipulator for a viapoints task with obstacles avoidance (batch formulation) | ✅ | ✅ | | |
| iLQR_manipulator_CP | iLQR with control primitives applied to a viapoint task with a manipulator (batch formulation) | ✅ | ✅ | | |
| iLQR_manipulator_object_affordance | iLQR applied to an object affordance planning problem with a planar manipulator, by considering object boundaries (batch formulation) | ✅ | ✅ | | |
| iLQR_manipulator_dynamics | iLQR applied to a reaching task by considering the dynamics of the manipulator | ✅ | ✅ | | |
-| iLQR_bimanual | iLQR applied to a planar bimanual robot for a tracking problem involving the center of mass (CoM) and the end-effector (batch formulation) | ✅ | | | |
+| iLQR_bimanual | iLQR applied to a planar bimanual robot for a tracking problem involving the center of mass (CoM) and the end-effector (batch formulation) | ✅ | ✅ | | |
| iLQR_bimanual_manipulability | iLQR applied to a planar bimanual robot problem with a cost on tracking a desired manipulability ellipsoid at the center of mass (batch formulation) | ✅ | | | |
| iLQR_bicopter | iLQR applied to a bicopter problem (batch formulation) | ✅ | ✅ | ✅ | |
| iLQR_car | iLQR applied to a car parking problem (batch formulation) | ✅ | ✅ | ✅ | |
diff --git a/cpp/CMakeLists.txt b/cpp/CMakeLists.txt
index 586fb033f0ee510de4bc92762de35c2642a5291e..afbb09900dae4a3df7d61a0dbb1e350cf4ed8e87 100644
--- a/cpp/CMakeLists.txt
+++ b/cpp/CMakeLists.txt
@@ -12,3 +12,4 @@ 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
diff --git a/cpp/src/iLQR_obstacle_GPIS.cpp b/cpp/src/iLQR_obstacle_GPIS.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8955d77bfff0ae86c46d426d8ca5480a06f53d93
--- /dev/null
+++ b/cpp/src/iLQR_obstacle_GPIS.cpp
@@ -0,0 +1,502 @@
+/*
+ iLQR applied to a 2D point-mass system reaching a target while avoiding
+ obstacles represented as Gaussian process implicit surfaces (GPIS)
+
+ Copyright (c) 2021 Idiap Research Institute, http://www.idiap.ch/
+ Written by Léane Donzé <leane.donze@epfl.ch>
+ 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 <Eigen/Core>
+#include <Eigen/Dense>
+#include <eigen3/unsupported/Eigen/KroneckerProduct>
+
+#include <iostream>
+#include <vector>
+
+namespace
+{
+ // define the parameters that influence the behaviour of the algorithm
+ struct Parameters
+ {
+ int num_iterations = 300; // maximum umber of iterations for iLQR
+ double dt = 1e-2; // time step size
+ int num_timesteps = 101; // number of datapoints
+
+ // definition of the viapoints, size <= num_timesteps
+ std::vector<Eigen::Vector3d> viapoints = {
+ { 0.9, 0.9, M_PI / 6 } //
+ };
+
+ // Control space dimension
+ int nbVarU = 2; //dx1, dx2
+
+ // State space diimension
+ int nbVarX = 2; //x1, x2
+
+ // initial configuration of the robot
+ std::vector<double> initial_state = { 0.3, 0.05};
+
+ // GPIS representation of obstacles
+ std::vector<double> p = {1.4, 1e-5}; //Thin-plate covariance function parameters
+ std::vector<std::vector<double>> x = { {0.2, 0.4, 0.6, -0.4, 0.6, 0.9},
+ {0.5, 0.5, 0.5, 0.8, 0.1, 0.6} };
+ std::vector<double> y = {-1, 0, 1, -1, -1, -1};
+
+ // Disc as gemoetric prior
+ double rc = 4e-1; // Radius of the disc
+ std::vector<double> xc = {0.55, 0.55}; // Location of the disc
+
+
+ double tracking_weight = 1e2; // tracking weight term
+ double obstacle_weight = 1e0; // obstacle weight term
+ double control_weight = 1e-3; // control weight term
+
+
+ };
+
+ struct Model
+ {
+ public:
+ /**
+ * initialize the model with the given parameter
+ * this calculates the matrices Su and Sx
+ */
+ Model(const Parameters ¶meters);
+
+ /**
+ * implementation of the iLQR algorithm
+ * this function calls the other functions as needed
+ */
+ Eigen::MatrixXd ilqr() const;
+
+ /**
+ * perform a trajectory rollout for the given initial joint angles and control commands
+ * return a joint angle trajectory
+ */
+ Eigen::MatrixXd rollout(const Eigen::VectorXd &initial_state, const Eigen::VectorXd &control_commands) const;
+
+ /**
+ * reaching function, called at each iLQR iteration
+ * calculates the error to each viapoint as well as the jacobians
+ */
+ std::pair<Eigen::MatrixXd, Eigen::MatrixXd> reach(const Eigen::MatrixXd &x) const;
+
+ /**
+ * Residuals f and Jacobians J for obstacle avoidance with GPIS representation
+ * (for all time steps)
+ */
+
+ std::tuple<Eigen::MatrixXd, Eigen::MatrixXd, std::vector<int>, std::vector<int> > avoid(const Eigen::MatrixXd &x) const;
+
+ /**
+ * convenience function to extract the viapoints from given joint angle trajectory
+ * also reshapes the viapoints to the expected matrix dimensions
+ */
+ Eigen::MatrixXd viapoints(const Eigen::MatrixXd &joint_angle_trajectory) const;
+
+ /**
+ * return the viapoint_timesteps_
+ * only used for plotting
+ */
+ const Eigen::VectorXi &viatimes() const;
+
+ /**
+ * implementation of the Matlab function "pdist2"
+ * which seems to have no equivalent in Eigen library
+ */
+ Eigen::MatrixXd pdist2(const Eigen::MatrixXd X, const Eigen::MatrixXd Y) const;
+
+ /**
+ * Error function
+ */
+ Eigen::MatrixXd substr(const Eigen::VectorXd x1, const Eigen::VectorXd x2) const;
+
+ /**
+ * Covariance function in GPIS
+ */
+ std::tuple<Eigen::MatrixXd, std::vector<Eigen::MatrixXd> > covFct(const Eigen::MatrixXd &x1, const Eigen::MatrixXd &x2, const Eigen::VectorXd p, bool flag_noiseObs) const;
+
+ /**
+ * Residuals f and Jacobians J for obstacle avoidance
+ * with GPIS representation (for a given time step)
+ */
+
+ std::tuple<Eigen::MatrixXd, Eigen::MatrixXd > GPIS(const Eigen::MatrixXd &x) const;
+
+
+ private:
+ /// parameters defined at runtime
+ Eigen::MatrixXd viapoints_; // internal viapoint representation
+ Eigen::VectorXi viapoint_timesteps_; // discrete timesteps of the viapoints, uniformly spread
+
+ double tracking_weight_;
+ double obstacle_weight_;
+ double r_; // control weight parameter
+
+ int nbVarU_; // Control space dimension (dx1, dx2)
+ int nbVarX_; // State space dimension (q1,q2,q3)
+ int num_timesteps_; // copy the number of timesteps for internal use from the Parameters
+ int num_iterations_; // maximum number of iterations for the optimization
+
+ Eigen::VectorXd initial_state_; // internal state
+ Eigen::MatrixXd control_weight_; // R matrix, penalizes the control commands
+ Eigen::MatrixXd precision_matrix_; // Q matrix, penalizes the state error
+ Eigen::MatrixXd mat_Su0_; // matrix for propagating the control commands for a rollout
+ Eigen::MatrixXd mat_Sx0_; // matrix for propagating the initial joint angles for a rollout
+ Eigen::MatrixXd mat_Su_; // matrix for propagating the control commands for a rollout at the viapoints
+
+
+ Eigen::VectorXd y_; // GPIS representation
+ Eigen::VectorXd Mu2_; // GPIS representation
+ Eigen::MatrixXd K_; // GPIS representation
+ Eigen::MatrixXd x_; // GPIS representation
+ Eigen::VectorXd p_ ; // GPIS representation
+
+ };
+
+ ////////////////////////////////////////////////////////////////////////////////
+ // implementation of the iLQR algorithm
+
+ Eigen::MatrixXd Model::ilqr() const
+ {
+ // initial commands, currently all zero
+ // can be modified if a better guess is available
+ Eigen::VectorXd control_commands = Eigen::VectorXd::Zero(nbVarU_* (num_timesteps_ - 1), 1);
+
+ int iter = 1;
+ for (; iter <= num_iterations_; ++iter) // run the optimization for the maximum number of iterations
+ {
+ std::cout << "iteration " << iter << ":" << std::endl;
+
+ // trajectory rollout, i.e. compute the trajectory for the given control commands starting from the initial state
+ // Sx * x0 + Su * u
+ Eigen::MatrixXd x = rollout(initial_state_, control_commands);
+
+ // Residuals f and Jacobians J (tracking objective)
+ auto [state_error, jacobian_matrix] = reach(viapoints(x));
+
+ // Residuals f and Jacobians J (avoidance objective)
+ auto [obstacle_error, obstacle_jacobian, idx, idt] = avoid(x);
+
+ Eigen::MatrixXd mat_Su2 = Eigen::MatrixXd::Zero(static_cast<int>(idx.size()), mat_Su0_.cols());
+ for (unsigned i = 0; i < idx.size(); ++i)
+ {
+ mat_Su2.row(i) = mat_Su0_.row(idx[i]);
+ }
+
+ // Gauss-Newton update
+ // (Su'*J'*Q*J*Su+Su2'*J2'*J2*Su2*param.q2 + R) \ (-Su'*J'*Q*f(:) - Su2'*J2'*f2(:) * param.q2 - u * param.r)
+ Eigen::MatrixXd control_gradient = (mat_Su_.transpose() * jacobian_matrix.transpose() * tracking_weight_ * jacobian_matrix * mat_Su_ //
+ + mat_Su2.transpose() * obstacle_jacobian.transpose() * obstacle_jacobian * mat_Su2 * obstacle_weight_ //
+ + control_weight_) //
+ .inverse() //
+ * (-mat_Su_.transpose() * jacobian_matrix.transpose() * tracking_weight_ * state_error //
+ - mat_Su2.transpose() * obstacle_jacobian.transpose() * obstacle_error * obstacle_weight_ //
+ - r_ * control_commands);
+
+ // calculate the cost of the current state
+ double current_cost = state_error.squaredNorm() * tracking_weight_ + obstacle_error.squaredNorm() * obstacle_weight_ + r_ * control_commands.squaredNorm();
+
+ std::cout << "\t cost = " << current_cost << std::endl;
+
+ // initial step size for the line search
+ double step_size = 1.0;
+ // line search, i.e. find the best step size for updating the control commands with the gradient
+ while (true)
+ {
+ // calculate the new control commands for the current step size
+ Eigen::MatrixXd tmp_control_commands = control_commands + control_gradient * step_size;
+
+ // calculate a trajectory rollout for the current step size
+ Eigen::MatrixXd tmp_x = rollout(initial_state_, tmp_control_commands);
+
+ // try reaching the viapoints with the current state
+ // we only need the state error here and can disregard the Jacobian, because we are only interested in the cost of the trajectory
+ Eigen::MatrixXd tmp_state_error = reach(viapoints(tmp_x)).first;
+
+ Eigen::MatrixXd tmp_obstacle_error = std::get<0>(avoid(tmp_x));
+
+ // resulting cost when updating the control commands with the current step size
+ double cost =
+ tmp_state_error.squaredNorm() * tracking_weight_ + tmp_obstacle_error.squaredNorm() * obstacle_weight_ + r_ * tmp_control_commands.squaredNorm();
+
+ // end the line search if the current steps size reduces the cost or becomes too small
+ if (cost < current_cost || step_size < 1e-3)
+ {
+ control_commands = tmp_control_commands;
+
+ break;
+ }
+
+ // reduce the step size for the next iteration
+ step_size *= 0.5;
+ }
+
+ std::cout << "\t step_size = " << step_size << std::endl;
+
+ // stop optimizing if the gradient update becomes too small
+ if ((control_gradient * step_size).norm() < 1e-2)
+ {
+ break;
+ }
+ }
+
+ std::cout << "iLQR converged in " << iter << " iterations" << std::endl;
+
+ return rollout(initial_state_, control_commands);
+ }
+ ////////////////////////////////////////////////////////////////////////////////
+ // implementation of all functions used in the iLQR algorithm
+
+ std::pair<Eigen::MatrixXd, Eigen::MatrixXd> Model::reach(const Eigen::MatrixXd &x) const
+ {
+ Eigen::MatrixXd state_error = x - viapoints_.topRows(2);
+
+ Eigen::MatrixXd jacobian_matrix = Eigen::MatrixXd::Identity(nbVarX_ * x.cols(), nbVarX_ * x.cols());
+
+ state_error = Eigen::Map<Eigen::MatrixXd>(state_error.data(), state_error.size(), 1);
+
+ return std::make_pair(state_error, jacobian_matrix);
+ }
+
+ std::tuple<Eigen::MatrixXd, Eigen::MatrixXd, std::vector<int>, std::vector<int> > Model::avoid(const Eigen::MatrixXd &x) const
+ {
+ auto [ftmp, Jtmp] = GPIS(x);
+
+ std::vector<double> fs;
+ std::vector<Eigen::MatrixXd> js;
+ std::vector<int> idx;
+ std::vector<int> idt;
+
+ int size = 0;
+
+ for (unsigned t = 0; t < x.cols(); ++ t)
+ {
+ if (ftmp(0,t) > 0)
+ {
+ fs.push_back(ftmp(0,t));
+ js.push_back(Jtmp.col(t).transpose());
+
+ size += 1;
+
+ for (unsigned j = 0; j < x.rows(); ++j)
+ {
+ idx.push_back(static_cast<int>(t * x.rows() + j));
+ }
+ idt.push_back(t);
+ }
+ }
+
+
+ Eigen::MatrixXd f = Eigen::Map<Eigen::MatrixXd>(fs.data(), static_cast<int>(fs.size()), 1);
+ Eigen::MatrixXd j = Eigen::MatrixXd::Zero(size, 2*size);
+
+ int r = 0, c = 0;
+ for (const Eigen::MatrixXd &ji : js)
+ {
+ j.block(r, c, ji.rows(), ji.cols()) = ji;
+ r += static_cast<int>(ji.rows());
+ c += static_cast<int>(ji.cols());
+ }
+
+ return std::make_tuple(f,j,idx,idt);
+
+ }
+
+ Eigen::MatrixXd Model::rollout(const Eigen::VectorXd &initial_state, const Eigen::VectorXd &control_commands) const
+ {
+ Eigen::MatrixXd x = mat_Su0_ * control_commands + mat_Sx0_ * initial_state;
+ x = Eigen::MatrixXd(Eigen::Map<Eigen::MatrixXd>(x.data(), nbVarX_, num_timesteps_));
+
+ return x;
+ }
+
+ Eigen::MatrixXd Model::viapoints(const Eigen::MatrixXd &joint_angle_trajectory) const
+ {
+ Eigen::MatrixXd via_joint_angles = Eigen::MatrixXd::Zero(joint_angle_trajectory.rows(), viapoint_timesteps_.size());
+
+ for (unsigned t = 0; t < viapoint_timesteps_.size(); ++t)
+ {
+ via_joint_angles.col(t) = joint_angle_trajectory.col(viapoint_timesteps_(t));
+ }
+
+ return via_joint_angles;
+ }
+
+ const Eigen::VectorXi &Model::viatimes() const
+ {
+ return viapoint_timesteps_;
+ }
+
+
+ Eigen::MatrixXd Model::pdist2(const Eigen::MatrixXd X, const Eigen::MatrixXd Y) const
+ {
+ Eigen::MatrixXd result(X.rows(), Y.rows());
+ for (unsigned i = 0; i < X.rows(); ++i)
+ {
+ for (unsigned j = 0; j < Y.rows(); ++j){
+ Eigen::VectorXd v1 = X.row(i);
+ Eigen::VectorXd v2 = Y.row(j);
+ result(i,j) = (v1-v2).norm();
+ }
+ }
+ return result;
+ }
+
+ Eigen::MatrixXd Model::substr(const Eigen::VectorXd x1, const Eigen::VectorXd x2) const
+ {
+ return x1.replicate(1,x2.size()) - x2.transpose().replicate(x1.size(), 1);
+ }
+
+ std::tuple<Eigen::MatrixXd, std::vector<Eigen::MatrixXd> > Model::covFct(const Eigen::MatrixXd &x1, const Eigen::MatrixXd &x2, const Eigen::VectorXd p, bool flag_noiseObs) const
+ {
+ // Thin plate covariance function (for 3D implicit shape)
+ Eigen::MatrixXd K = pow(12,-1)*(2*pdist2(x1.transpose(),x2.transpose()).array().pow(3) - 3*p(0)*pdist2(x1.transpose(), x2.transpose()).array().pow(2) + pow(p(0),3)); // Kernel
+ Eigen::MatrixXd dK1 = pow(12,-1)*(6*pdist2(x1.transpose(),x2.transpose()).cwiseProduct(substr(x1.row(0).transpose(), x2.row(0))) - 6*p(0)*substr(x1.row(0).transpose(), x2.row(0))); // Derivative along x1
+ Eigen::MatrixXd dK2 = pow(12,-1)*(6*pdist2(x1.transpose(),x2.transpose()).cwiseProduct(substr(x1.row(1).transpose(), x2.row(1))) - 6*p(0)*substr(x1.row(1).transpose(), x2.row(1))); // Derivative along x2
+
+ std::vector<Eigen::MatrixXd> dK;
+ dK.push_back(dK1);
+ dK.push_back(dK2);
+
+ if (flag_noiseObs == true)
+ {
+ K = K + p(1)*Eigen::MatrixXd::Identity(x1.cols(), x2.cols()); //Consideration of noisy observation y
+ }
+ return std::make_pair(K, dK);
+ }
+
+ std::tuple<Eigen::MatrixXd, Eigen::MatrixXd > Model::GPIS(const Eigen::MatrixXd &x) const
+ {
+ auto [K, dK] = covFct(x, x_, p_, false);
+ Eigen::MatrixXd f = (K*K_.inverse()*y_).transpose(); //GPR with Mu=0
+ Eigen::MatrixXd J(x.rows(), x.cols());
+ J.row(0) = (dK[0]*K_.inverse() * (y_-Mu2_)).transpose();
+ J.row(1) = (dK[1]*K_.inverse() * (y_-Mu2_)).transpose();
+
+ // Reshape gradients
+ Eigen::MatrixXd a = f.cwiseMax(0); //Amplitude
+ J = 1e2*(a.array().tanh().replicate(2,1).cwiseProduct(J.array()).cwiseQuotient(J.array().square().colwise().sum().sqrt().replicate(2,1))); // Vector moving away from interior of shape
+
+ return std::make_tuple(f, J);
+
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ ////////////////////////////////////////////////////////////////////////////////
+ // precalculate matrices used in the iLQR algorithm
+
+ Model::Model(const Parameters ¶meters)
+ {
+ int num_viapoints = static_cast<int>(parameters.viapoints.size());
+
+ nbVarU_ = parameters.nbVarU;
+ nbVarX_= parameters.nbVarX;
+
+ r_ = parameters.control_weight;
+ tracking_weight_ = parameters.tracking_weight;
+ obstacle_weight_ = parameters.obstacle_weight;
+
+ num_timesteps_ = parameters.num_timesteps;
+ num_iterations_ = parameters.num_iterations;
+
+ viapoints_ = Eigen::MatrixXd::Zero(3, num_viapoints);
+ for (unsigned t = 0; t < parameters.viapoints.size(); ++t)
+ {
+ viapoints_.col(t) = parameters.viapoints[t];
+ }
+
+ initial_state_ = Eigen::VectorXd::Zero(nbVarX_);
+ for (unsigned i = 0; i < parameters.initial_state.size(); ++i)
+ {
+ initial_state_(i) = parameters.initial_state[i];
+ }
+
+ viapoint_timesteps_ = Eigen::VectorXd::LinSpaced(num_viapoints + 1, 0, num_timesteps_ - 1).bottomRows(num_viapoints).array().round().cast<int>();
+
+ control_weight_ = r_ * Eigen::MatrixXd::Identity((num_timesteps_ - 1) * nbVarU_, (num_timesteps_ - 1) * nbVarU_);
+ precision_matrix_ = tracking_weight_* Eigen::MatrixXd::Identity(nbVarX_ * num_viapoints, nbVarX_ * num_viapoints);
+
+ Eigen::MatrixXi idx = Eigen::VectorXi::LinSpaced(nbVarX_, 0, nbVarX_ - 1).replicate(1, num_viapoints);
+
+ for (unsigned i = 0; i < idx.rows(); ++i)
+ {
+ idx.row(i) += Eigen::VectorXi((viapoint_timesteps_.array()) * nbVarX_).transpose();
+ }
+
+ mat_Su0_ = Eigen::MatrixXd::Zero(nbVarX_* (num_timesteps_), nbVarX_* (num_timesteps_ - 1));
+ mat_Su0_.bottomRows(nbVarX_ * (num_timesteps_ - 1)) = kroneckerProduct(Eigen::MatrixXd::Ones(num_timesteps_ - 1, num_timesteps_ - 1), //
+ parameters.dt * Eigen::MatrixXd::Identity(nbVarX_, nbVarX_))
+ .eval()
+ .triangularView<Eigen::Lower>();
+ mat_Sx0_ = kroneckerProduct(Eigen::MatrixXd::Ones(num_timesteps_, 1), //
+ Eigen::MatrixXd::Identity(nbVarX_, nbVarX_))
+ .eval();
+
+ mat_Su_ = Eigen::MatrixXd::Zero(idx.size(), nbVarX_* (num_timesteps_ - 1));
+ for (unsigned i = 0; i < idx.size(); ++i)
+ {
+ mat_Su_.row(i) = mat_Su0_.row(idx(i));
+ }
+
+ y_ = Eigen::VectorXd::Zero(static_cast<int>(parameters.y.size()));
+ for (unsigned i = 0; i < parameters.y.size(); ++i)
+ {
+ y_(i) = parameters.y[i];
+ }
+
+ Eigen::MatrixXd S = pow(parameters.rc, -2) * Eigen::MatrixXd::Identity(2,2);
+ x_ = Eigen::MatrixXd::Zero(parameters.x.size(), parameters.x[0].size());
+ for (unsigned i = 0; i < parameters.x.size(); ++i)
+ {
+ for (unsigned j = 0; j < parameters.x[0].size(); ++j){
+ x_(i,j) = parameters.x[i][j];
+ }
+ }
+
+ p_ = Eigen::VectorXd::Zero(static_cast<int>(parameters.y.size()));
+ for (unsigned i = 0; i < parameters.p.size(); ++i)
+ {
+ p_(i) = parameters.p[i];
+ }
+
+ Eigen::VectorXd xc = Eigen::VectorXd::Zero(static_cast<int>(parameters.xc.size()));
+ for (unsigned i = 0; i < parameters.xc.size(); ++i)
+ {
+ xc(i) = parameters.xc[i];
+ }
+
+ Mu2_ = 0.5* parameters.rc* (Eigen::MatrixXd::Ones(x_.cols(), x_.cols()) - (x_ - xc.replicate(1,x_.cols())).transpose()*S //
+ *(x_ - xc.replicate(1,x_.cols()))).diagonal();
+
+ K_ = std::get<0>(covFct(x_,x_,p_,true));
+
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+}
+
+int main()
+{
+ Parameters parameters;
+ Model model(parameters); // initialize the model with the parameters that were defined at the top
+
+ Eigen::MatrixXd trajectory = model.ilqr(); // calculate the iLQR solution
+
+ return 0;
+}
\ No newline at end of file
diff --git a/doc/rcfs.pdf b/doc/rcfs.pdf
index 181057d09186781c22d5f9034b633d02a6d193c9..09e980a1ea28dc6445bfc3d459d09351fd5f62a6 100644
Binary files a/doc/rcfs.pdf and b/doc/rcfs.pdf differ
diff --git a/doc/rcfs.tex b/doc/rcfs.tex
index 17f3ef1c8a20f8b562589fdcac8c6dc18e8702d6..3298338a459d93caf084a9235e479acec600077a 100644
--- a/doc/rcfs.tex
+++ b/doc/rcfs.tex
@@ -1,5 +1,6 @@
\documentclass[10pt,a4paper,twocolumn]{article}
\usepackage{graphicx,amsmath,amssymb,bm,xcolor,soul,nicefrac,listings,algorithm2e,dsfont}
+%\usepackage[hidelinks]{hyperref}
%pseudocode
\newcommand\mycommfont[1]{\footnotesize\ttfamily\textcolor{lightgray}{#1}}
@@ -420,7 +421,7 @@ where $\Delta$ is a small value for the approximation of the derivatives.
\filename{FD.*}
\end{flushright}
-The dynamic equation of a planar robot with an arbitrary number of links can be derived using the Lagrangian formulation. The first step to use the Lagrangian method is to represent the kinetic and potential energies of the robot as functions of the joint angles. We assume that all masses are located at the end of each link, and we neglect the terms that contain rotational energies (i.e, zero moments of inertia). To do this, we exploit the formulation derived in Section \ref{sec:FK} without the orientation part, so the position of the j-\emph{th} mass can be written as
+The dynamic equation of a planar robot with an arbitrary number of links can be derived using the Lagrangian formulation. The first step to use this method is to represent the kinetic and potential energies of the robot as functions of generalized coordinates, which are joint angles in this case. We assume that all masses are located at the end of each link, and we neglect the terms that contain rotational energies (i.e, zero moments of inertia). To do this, we exploit the formulation derived in Section \ref{sec:FK} without the orientation part, so the position of the j-\emph{th} mass can be written as
\begin{equation}
\bm{f}_j = \begin{bmatrix}
f_{j,1} \\ f_{j,2}
@@ -434,7 +435,7 @@ where
c_k = \cos(\sum_{i=1}^{k} x_i), \ \ \ s_k = \sin(\sum_{i=1}^{k} x_i),
\end{align}
-so the j-\emph{th} mass point velocity can be calculated as
+whose corresponding velocity can be calculated as
\begin{equation*}
\bm{\dot{f}}_j = \begin{bmatrix}
-\sum_{k=1}^j l_k (\sum_{i=1}^{k} \dot{x}_i) s_k \\ \sum_{k=1}^j l_k (\sum_{i=1}^{k} \dot{x}_i) c_k
@@ -476,10 +477,10 @@ where $N$ is the number of links. Using the Euler-Lagrange equation, we can writ
u_z = \sum_{j=z}^N \Big(\frac{d}{dt} \frac{\partial T_j}{\partial \dot{q}_z} - \frac{\partial T_j}{\partial q_z} + \frac{\partial U_j}{\partial q_z}\Big),
\label{eq:Lag_with_generalized_forces}
\end{equation}
-where $u_z$ is the generalized force related to $q_z$. This force can be found from the corresponding generalized work $w_z$ as
-\begin{equation*}\label{eq:generalized_work}
+where $u_z$ is the generalized force related to $q_z$. This force can be found from the corresponding generalized work $w_z$, which can be described by
+\begin{equation}\label{eq:generalized_work}
w_z = \begin{cases} (\tau_{z}-\tau_{z+1})\dot{q}_z & z<N \\ \tau_{N}\dot{q}_N & z=N\end{cases},
-\end{equation*}
+\end{equation}
where $\tau_z$ is the torque applied at z-\emph{th} joint. The fact that we need subtraction in the first line of \eqref{eq:generalized_work} is that when $\tau_{z+1}$ is applied on link $l_{z+1}$, its reaction is applied on link $l_z$, except for the last joint where there is no reaction from the proceeding links. Please note that if we had used relative angles in our formulation, we did not need to consider this reaction as it will be cancelled by itself at link $z+1$ (as $\dot{q}_{z+1} = \dot{q}_z + \dot{x}_{z+1}$). That said, generalized forces can be defined as
\begin{equation*}
u_z = \frac{\partial w_z}{\partial \dot{q}_z} = \begin{cases} (\tau_{z}-\tau_{z+1}) & z<N \\ \tau_{N} & z=N\end{cases}.
@@ -2167,14 +2168,14 @@ which can then be converted to a discrete form with \eqref{eq:AdAc}.
\filename{iLQR\_manipulator\_dynamics.*}
\end{flushright}
-The nonlinear dynamic equation of a planar robot presented in Section \ref{sec:FD} can be expressed as (For simplicity we start with \eqref{eq:matix_form_dynamic_abs_angle_net_forces}, and at the end, we will describe how the result can be converted to the other choice of variables.)
-\begin{equation*}
+The nonlinear dynamic equation of a planar robot presented in Section \ref{sec:FD} can be expressed as
+\begin{equation}\label{eq:dis_dynamic}
\begin{bmatrix} \bm{q}_{t+1} \\ \dot{\bm{q}}_{t+1}
\end{bmatrix}= \begin{bmatrix} \bm{q}_{t} + \dot{\bm{q}}_{t} dt \\ \dot{\bm{q}}_{t} + \bm{M}^{-1}\Big(\bm{u} + \bm{g} + \bm{C}\text{diag}(\bm{\dot{q}}_t)\bm{\dot{q}_t}\Big) dt
\end{bmatrix}.
-\end{equation*}
+\end{equation}
-The iLQR method requires this equation to be linearized around the current states and inputs.The corresponding $\bm{A}_t$ and $\bm{B}_t$ can be found according to \eqref{eq:DS} as
+For simplicity we start with \eqref{eq:matix_form_dynamic_abs_angle_net_forces}, and at the end, we will describe how the result can be converted to the other choice of variables. The iLQR method requires \eqref{eq:dis_dynamic} to be linearized around the current states and inputs.The corresponding $\bm{A}_t$ and $\bm{B}_t$ can be found according to \eqref{eq:DS} as
\begin{align}
\bm{A}_t =& \begin{bmatrix}
\bm{I} & \bm{I} dt \\ \bm{A}_{21}dt & \bm{I}+2 \bm{M}^{-1} \bm{C} \text{diag}(\dot{\bm{
@@ -2185,7 +2186,7 @@ q}}_t) dt
\end{bmatrix}.\nonumber
\end{align}
-$\bm{A}_{21} = \frac{\partial \ddot{\bm{q}}_t}{\partial \bm{q}_t}$ can be found by taking the derivation of \eqref{eq:matix_form_dynamic_abs_angle_net_forces} w.r.t. $\bm{q}_t$. The $p$-\emph{th} column of $\bm{A}_{21}$ is the partial derivation of $\ddot{\bm{q}}_t$ w.r.t. $q_p^t$ which can be formulated as
+$\bm{A}_{21} = \frac{\partial \ddot{\bm{q}}_t}{\partial \bm{q}_t}$ can be found by taking the derivation of \eqref{eq:matix_form_dynamic_abs_angle_net_forces} w.r.t. $\bm{q}_t$. The $p$-\emph{th} column of $\bm{A}_{21}$ is the partial derivation of $\ddot{\bm{q}}_t$ w.r.t. $p$-emph{th} joint angle at time $t$ $q_p^t$ which can be formulated as
\begin{multline*}
\frac{\partial \ddot{\bm{q}}_t}{\partial q_p^t} = \frac{\partial \bm{M}^{-1}}{\partial q_p^t} \Big(\bm{u} + \bm{g} + \bm{C}\text{diag}(\bm{\dot{q}}_t)\bm{\dot{q}}_t\Big) + \\
\bm{M}^{-1} \Big(\frac{\partial \bm{g}}{\partial q_p^t} +
@@ -2232,7 +2233,7 @@ where
\end{cases}.
\end{equation*}
-According to Sec. \ref{sec:iLQRbatch}, we can concatenate the results for all time steps as
+According to Section \ref{sec:iLQRbatch}, we can concatenate the results for all time steps as
\begin{equation}\label{eq:transfer_matrix_abs_ang_gen_force}
\begin{bmatrix}
\Delta \bm{q}_1 \\ \Delta \bm{\dot{q}}_1 \\ \vdots \\ \Delta \bm{q}_T \\ \Delta \bm{\dot{q}}_T
@@ -2263,9 +2264,9 @@ so \eqref{eq:transfer_matrix_abs_ang_gen_force} can be rewritten as
\begin{equation*}
\begin{bmatrix}
\Delta \bm{x}_1 \\ \Delta \bm{\dot{x}}_1 \\ \vdots \\ \Delta \bm{x}_T \\ \Delta \bm{\dot{x}}_T
-\end{bmatrix}, = \bm{S}_{\tau}^x \begin{bmatrix}
+\end{bmatrix} = \bm{S}_{\tau}^x \begin{bmatrix}
\Delta \bm{\tau}_1 \\ \Delta \bm{\tau}_2 \\ \vdots \\ \Delta \bm{\tau}_{T-1}
-\end{bmatrix}, = \bm{T}_x^{-1} \bm{S}_u^q \bm{T}_u \begin{bmatrix}
+\end{bmatrix} = \bm{T}_x^{-1} \bm{S}_u^q \bm{T}_u \begin{bmatrix}
\Delta \bm{\tau}_1 \\ \Delta \bm{\tau}_2 \\ \vdots \\ \Delta \bm{\tau}_{T-1}
\end{bmatrix}.
\end{equation*}
@@ -2359,7 +2360,7 @@ A single integrator is simply defined as $\bm{x}_{t+1} = \bm{x}_t + \bm{u}_t \De
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section{Derivation of motion equation for a planar robot}\label{sec:FD_derivation}
- We derive each element in \eqref{eq:Lag_with_generalized_forces} individually for $j \geq z$, then combine them altogether to derive the dynamic equation of the system. In this regard, for the first element in \eqref{eq:Lag_with_generalized_forces}, we can write
+We derive each element in \eqref{eq:Lag_with_generalized_forces} individually for $j \geq z$, then combine them altogether to derive the dynamic equation of the system. In this regard, for the first element in \eqref{eq:Lag_with_generalized_forces}, we can write
\begin{align*}
\frac{\partial T_j}{\partial \dot{q}_z} &= m_j \Big(\frac{\partial \dot{f}_{j,1}}{\partial \dot{q}_z} \dot{f}_{j,1}
+ \frac{\partial \dot{f}_{j,2}}{\partial \dot{q}_z} \dot{f}_{j,2}\Big) = \\
@@ -2388,7 +2389,7 @@ and finally, the potential energy term can be calculated as
\frac{\partial U_j}{\partial q_z} = m_j g l_z c_z.
\end{equation*}
-The z-\emph{th} general force can be found by substituting the derived terms to \eqref{eq:Lag_with_generalized_forces} as
+The z-\emph{th} generalized force can be found by substituting the derived terms to \eqref{eq:Lag_with_generalized_forces} as
\begin{multline*}
u_z = \sum_{j=z}^N \Big(m_j \Big(\Big(- l_z \dot{q}_z c_z\Big) \dot{r}_{j,1} - \Big(l_z s_k\Big) \ddot{r}_{j,1} - \Big(l_z \dot{q}_z s_z\Big) \dot{r}_{j,2} \\ + \Big(l_z c_z\Big) \ddot{r}_{j,2}\Big) - m_j \Big(\Big(-l_z \dot{q}_z c_z\Big)\dot{r}_{j,1} - \Big(l_z \dot{q}_z s_z\Big) \dot{r}_{j,2}\Big) \\ + m_j g l_z c_z\Big)\\ = \sum_{j=z}^N \Big(m_j \Big(\Big(-l_z s_z\Big) \ddot{r}_{j,1} + \Big(l_z c_z\Big) \ddot{r}_{j,2}\Big) +m_j g l_z c_z\Big)\\
= \sum_{j=z}^N \Big(m_j \Big(\Big(-l_z s_z\Big) \Big(-\sum_{k=1}^j l_k \ddot{q}_k s_k -\sum_{k=1}^j l_k \dot{q}_k^2 c_k\Big) + \\ \Big(l_z c_z\Big) \Big(\sum_{k=1}^j l_k \ddot{q}_k c_k- \sum_{k=1}^j l_k \dot{q}_k^2 s_k\Big)\Big) +m_j g l_z c_z \Big) \\= \sum_{j=z}^{N}m_j \Big( \sum_{k=1}^{j} l_z l_k c_{z-k}\ddot{q}_k + \sum_{k=1}^{j} l_z l_k s_{z-k}\dot{q}_k^2\Big) + \sum_{j=z}^{N}m_j g l_z c_z,
diff --git a/matlab/IK_nullspace.m b/matlab/IK_nullspace.m
index f2cb4990e028bc871907929c0eab9197fafc692b..5b330e2ba28de7e1e78d480e9fa013a10e5af9e9 100644
--- a/matlab/IK_nullspace.m
+++ b/matlab/IK_nullspace.m
@@ -68,15 +68,15 @@ for t=1:param.nbIter
%1. Determine the first step size
alpha1 = 1;
- cost10 = (param.fh(1:2) - f(1:2))'*(param.fh(1:2) - f(1:2)); %current cost
- cost1 = 5*cost10; %initialize with something big
+ cost10 = (param.fh(1:2) - f(1:2))' * (param.fh(1:2) - f(1:2)); %current cost
+ cost1 = 5 * cost10; %initialize with something big
dxp = pinvJp * dfp; %search direciton for first cost
while cost1 > cost10 + 1e-4 %1e-4 is the tolerance
- dxp_try = alpha1*dxp;
+ dxp_try = alpha1 * dxp;
x_next = x + dxp_try;
f = fkin(x_next, param);
- cost1 = (param.fh(1:2) - f(1:2))'*(param.fh(1:2) - f(1:2));
- alpha1 = alpha1*param.alpha_fac;
+ cost1 = (param.fh(1:2) - f(1:2))' * (param.fh(1:2) - f(1:2));
+ alpha1 = alpha1 * param.alpha_fac;
end
dxp = dxp_try;
@@ -86,11 +86,11 @@ for t=1:param.nbIter
%2. Determine the second step size
alpha2 = 1;
- cost10 = (param.fh(1:2) - f(1:2))'*(param.fh(1:2) - f(1:2));%current cost1
- cost20 = (param.fh(3) - f(3))'*(param.fh(3) - f(3));%current cost2
+ cost10 = (param.fh(1:2) - f(1:2))' * (param.fh(1:2) - f(1:2));%current cost1
+ cost20 = (param.fh(3) - f(3))' * (param.fh(3) - f(3));%current cost2
- cost1 = 5*cost10 + 1e1; %initialize with something big
- cost2 = 5*cost20; %initialize with something big
+ cost1 = 5 * cost10 + 1e1; %initialize with something big
+ cost2 = 5 * cost20; %initialize with something big
JoNp = Jo * Np;
pinvJoNp = JoNp' / (JoNp * JoNp' + eye(1) * param.mu_o);
@@ -99,21 +99,21 @@ for t=1:param.nbIter
%decrease alpha2 until the cost 1 and cost2 are lower or the same
%(with some tolerance)
while (cost1 > cost10 + 1e-8) || (cost2 > cost20 + 1e-4) %1e-8 is the tolerance
- dxo_try = alpha2*dxo;
+ dxo_try = alpha2 * dxo;
x_next = x + dxp + dxo_try;
f = fkin(x_next, param);
- cost1 = (param.fh(1:2) - f(1:2))'*(param.fh(1:2) - f(1:2));
- cost2 = (param.fh(3) - f(3))'*(param.fh(3) - f(3));
- alpha2 = alpha2*param.alpha_fac;
+ cost1 = (param.fh(1:2) - f(1:2))' * (param.fh(1:2) - f(1:2));
+ cost2 = (param.fh(3) - f(3))' * (param.fh(3) - f(3));
+ alpha2 = alpha2 * param.alpha_fac;
end
dxo = dxo_try;
dx = dxp + dxo;
x = x + dx ;%* dt; %Update pose
- fprintf('Costp: %f, Costo: %f, alpha1: %f, alpha2:%f\n', ...
- (param.fh(1:2) - f(1:2))'*(param.fh(1:2) - f(1:2)), ...
- (param.fh(3) - f(3))*(param.fh(3) - f(3)), alpha1, alpha2);
+ fprintf('Cost_p: %f, Cost_o: %f, alpha1: %f, alpha2:%f\n', ...
+ (param.fh(1:2) - f(1:2))' * (param.fh(1:2) - f(1:2)), ...
+ (param.fh(3) - f(3)) * (param.fh(3) - f(3)), alpha1, alpha2);
%Plot the robot
fs = fkin0(x, param);
@@ -126,7 +126,7 @@ fs = fkin0(x, param);
plot(fs(1,:), fs(2,:), '-','linewidth',4,'color',[.4 .4 .4]);
axis equal;
waitfor(h);
-
+end
%%%%%%%%%%%%%%%%%%%%%%
%Forward kinematics for all robot articulations (in robot coordinate system)
@@ -168,5 +168,3 @@ function J = Jkin(x, l)
F2 = fkin(X + eye(D) * e, l);
J = (F2 - F1) / e;
end
-
-end
\ No newline at end of file
diff --git a/python/FD.py b/python/FD.py
index f0c00843154fb705ab2329853140067f4c200e61..0adadbd79292174121300efdbe23fbd617e7684d 100644
--- a/python/FD.py
+++ b/python/FD.py
@@ -29,8 +29,11 @@ import matplotlib.animation as animation
# Helper functions
# ===============================
+# NB: Because of the use of animation, we need to set param as a global variable
+
# Forward Kinematics for all joints
def fkin0(x):
+ global param
T = np.tril(np.ones([param.nbVarX,param.nbVarX]))
T2 = np.tril(np.matlib.repmat(param.l,len(x),1))
f = np.vstack((
@@ -45,12 +48,14 @@ def fkin0(x):
# Initialization of the plot
def init():
+ global param
ax.set_xlim(-np.sum(param.l) - 0.1, np.sum(param.l) + 0.1)
ax.set_ylim(-np.sum(param.l) - 0.1, np.sum(param.l) + 0.1)
return ln1, ln2
# Updating the values in the plot
def animate(i):
+ global param
f = fkin0(x[0:param.nbVarX,i])
ln1.set_data(f[:,0], f[:,1])
ln2.set_data(f[:,0], f[:,1])
diff --git a/python/FD_recursive.py b/python/FD_recursive.py
index d92f1b1d898ca10670ca11b9fc79541877c40ace..d8af64381de30fe7c7e331477b34e66af79b2ee5 100644
--- a/python/FD_recursive.py
+++ b/python/FD_recursive.py
@@ -29,8 +29,11 @@ import matplotlib.animation as animation
# Helper functions
# ===============================
+# NB: Because of the use of animation, we need to set param as a global variable
+
# Forward Kinematics for all joints
def fkin0(x):
+ global param
T = np.tril(np.ones([param.nbVarX,param.nbVarX]))
T2 = np.tril(np.matlib.repmat(param.l,len(x),1))
f = np.vstack((
@@ -45,12 +48,14 @@ def fkin0(x):
# Initialization of the plot
def init():
+ global param
ax.set_xlim(-np.sum(param.l) - 0.1, np.sum(param.l) + 0.1)
ax.set_ylim(-np.sum(param.l) - 0.1, np.sum(param.l) + 0.1)
return ln1, ln2
# Updating the values in the plot
def animate(i):
+ global param
f = fkin0(x[0:param.nbVarX,i])
ln1.set_data(f[:,0], f[:,1])
ln2.set_data(f[:,0], f[:,1])
diff --git a/python/LQT.py b/python/LQT.py
index 8008d98cf9b099b0d3972642dee70d651708831c..513a5b9e7358c2273801b26cbcd71824b942fa85 100644
--- a/python/LQT.py
+++ b/python/LQT.py
@@ -28,9 +28,9 @@ import matplotlib.pyplot as plt
# ===============================
param = lambda: None # Lazy way to define an empty class in python
param.dt = 1e-2 # Time step length
-param.nbPoints = 3
-param.nbDeriv = 2
-param.nbVarPos = 2
+param.nbPoints = 3 # Number of targers
+param.nbDeriv = 2 # Order of the dynamical system
+param.nbVarPos = 2 # Number of position variable
param.nbData = 100 # Number of datapoints
param.rfactor = 1e-9 # Control weight term
diff --git a/python/LQT_CP.py b/python/LQT_CP.py
index 6fd37a650f4f06a920da12bf9bddc5e291de2147..1108006123474ec96d9b36b3573cd91abdcb1bb1 100644
--- a/python/LQT_CP.py
+++ b/python/LQT_CP.py
@@ -60,9 +60,9 @@ def build_phi_fourier(nb_data, nb_fct):
param = lambda: None # Lazy way to define an empty class in python
param.dt = 1e-2 # Time step length
-param.nbPoints = 3
-param.nbDeriv = 2
-param.nbVarPos = 2
+param.nbPoints = 3 # Number of target points
+param.nbDeriv = 2 # Order of the dynamics model
+param.nbVarPos = 2 # Number of position variable
param.nbData = 100 # Number of datapoints
param.nbFct = 3 # Number of basis function
param.rfactor = 1e-9 # Control weight term
diff --git a/python/LQT_recursive.py b/python/LQT_recursive.py
index 719790dd5cf0a1dee34b8792dcac00105c14ffe0..87ccc0b0bb5512e1da8363ca9abc3b8aa0f825a2 100644
--- a/python/LQT_recursive.py
+++ b/python/LQT_recursive.py
@@ -30,9 +30,9 @@ from matplotlib.cm import get_cmap
param = lambda: None # Lazy way to define an empty class in python
param.dt = 1e-2 # Time step length
-param.nbPoints = 2
-param.nbDeriv = 2
-param.nbVarPos = 2
+param.nbPoints = 2 # Number of target point
+param.nbDeriv = 2 # Order of the dynamical system
+param.nbVarPos = 2 # Number of position variable
param.nbData = 100 # Number of datapoints
param.rfactor = 1e-9 # Control weight term
diff --git a/python/LQT_recursive_LS.py b/python/LQT_recursive_LS.py
index 780ee17652ad2a070211ba10745bb19b0b9cd450..d6d8e59196fa80712a58ffe19a64ae04a08115ad 100644
--- a/python/LQT_recursive_LS.py
+++ b/python/LQT_recursive_LS.py
@@ -28,9 +28,9 @@ import matplotlib.pyplot as plt
# ===============================
param = lambda: None # Lazy way to define an empty class in python
param.dt = 1e-2 # Time step length
-param.nbPoints = 1
-param.nbDeriv = 2
-param.nbVarPos = 2
+param.nbPoints = 1 # Number of target point
+param.nbDeriv = 2 # Order of the dynamical system
+param.nbVarPos = 2 # Number of position variable
param.nbData = 100 # Number of datapoints
param.rfactor = 1e-9 # Control weight term
diff --git a/python/LQT_recursive_LS_multiAgents.py b/python/LQT_recursive_LS_multiAgents.py
index 9e80bead31f6c7df5f6b94771f40091e961234f2..34073e2ae5947b78112a7ebed535811c8c41b5df 100644
--- a/python/LQT_recursive_LS_multiAgents.py
+++ b/python/LQT_recursive_LS_multiAgents.py
@@ -29,9 +29,9 @@ import matplotlib.pyplot as plt
param = lambda: None # Lazy way to define an empty class in python
param.dt = 1e-2 # Time step length
-param.nbPoints = 1
-param.nbDeriv = 2
-param.nbVarPos = 2
+param.nbPoints = 1 # Number of target points
+param.nbDeriv = 2 # Order of the dynamical system
+param.nbVarPos = 2 # Number of position variable
param.nbData = 100 # Number of datapoints
param.rfactor = 1e-9 # Control weight term
@@ -46,29 +46,37 @@ via_point_timing = np.linspace(0, param.nbData-1, param.nbPoints+1)
for tv_ in via_point_timing[1:]:
tv = int(tv_)
tv_slice = slice(tv*nb_var, (tv+1)*nb_var)
+
X_d_tv = np.concatenate((np.random.uniform(-1.0, 1.0, size=2 * param.nbVarPos), np.zeros(2 * param.nbVarPos)))
+
via_points.append(X_d_tv[:2*param.nbVarPos])
print("via point: ", X_d_tv)
+
Q_tv = np.eye(nb_var-1)
if tv < param.nbData-1:
Q_tv[2*param.nbVarPos:, 2*param.nbVarPos:] = 1e-6*np.eye(2*param.nbVarPos) # Don't track velocities on the way
+
Q_inv = np.zeros((nb_var, nb_var))
Q_inv[:nb_var-1, :nb_var-1] = np.linalg.inv(Q_tv) + np.outer(X_d_tv, X_d_tv)
Q_inv[:nb_var-1, -1] = X_d_tv
Q_inv[-1, :nb_var-1] = X_d_tv
Q_inv[-1, -1] = 1
+
Q[tv_slice, tv_slice] = np.linalg.inv(Q_inv)
# Construct meeting point constraint at half of the horizon
tv = int(0.5 * param.nbData)
tv_slice = slice(tv*nb_var, (tv+1)*nb_var)
Q_tv = np.eye(nb_var-1)
+
# Off-diagonal to tie position
Q_tv[:param.nbVarPos, param.nbVarPos:2*param.nbVarPos] = - (1.0 - 1e-6) * np.eye(param.nbVarPos)
Q_tv[param.nbVarPos:2*param.nbVarPos, :param.nbVarPos] = - (1.0 - 1e-6) * np.eye(param.nbVarPos)
+
# Off-diagonal to tie velocity
Q_tv[2*param.nbVarPos:3*param.nbVarPos, 3*param.nbVarPos:4*param.nbVarPos] = - (1.0 - 1e-6) * np.eye(param.nbVarPos)
Q_tv[3*param.nbVarPos:4*param.nbVarPos, 2*param.nbVarPos:3*param.nbVarPos] = - (1.0 - 1e-6) * np.eye(param.nbVarPos)
+
#Q_tv[2*param.nbVarPos:, 2*param.nbVarPos:] = 1e-6*np.eye(2*param.nbVarPos) # Don't track absolute velocities
Q_inv = np.zeros((nb_var, nb_var))
Q_inv[:nb_var-1, :nb_var-1] = np.linalg.inv(Q_tv)
diff --git a/python/iLQR_bicopter.py b/python/iLQR_bicopter.py
index 82451eb93d9ef5a3294aefdc0faac6c85249f430..eef8557f40c4ef37a03fdb7e84522fe866d2a81f 100644
--- a/python/iLQR_bicopter.py
+++ b/python/iLQR_bicopter.py
@@ -47,7 +47,7 @@ def f_reach(param, x_init, Us):
Bs[i] = get_B(param, Xs[i])
Xs[i+1] = step(param, Xs[i], Us[i])
- return Xs,get_system_matrix(As,Bs)
+ return Xs,get_system_matrix(param,As,Bs)
# Get df(x,u)/dx
def get_A(param, x, u):
@@ -67,7 +67,7 @@ def get_B(param, x):
return B
# Build Su
-def get_system_matrix(As,Bs):
+def get_system_matrix(param,As,Bs):
Su = np.zeros((param.nbDoF*param.nbSteps, 2*(param.nbSteps-1)))
for j in range(param.nbSteps-1):
Su[(j+1)*param.nbDoF:(j+2)*param.nbDoF, j*2:(j+1)*2] = Bs[j]
@@ -89,18 +89,17 @@ def plot_bicopter(param, x, alpha=1.0):
# Parameters
# ===============================
param = lambda: None # Lazy way to define an empty class in python
-param.size = 0.2
-param.mass = 1.0
-param.g = 9.81
-param.dt = 1e-2
+param.size = 0.2 # Size of the bicopter
+param.mass = 1.0 # Mass of the bicopter
+param.g = 9.81 # Gravity
+param.dt = 1e-2 # Delta t
param.x_d = np.array([-2, 1, 0 ]) # Desired_position
-param.precision = 1e3
-param.r = 1e-3
-param.horizon = 1.0
-param.nbIter = 30
-
-param.nbSteps = int(param.horizon / param.dt)
-param.nbDoF = 6
+param.precision = 1e3 # Precision weight
+param.r = 1e-3 # Control weight term
+param.horizon = 1.0 # Horizon of the control problem
+param.nbIter = 30 # Number of iLQR iteration
+param.nbSteps = int(param.horizon / param.dt) # Number of timestep
+param.nbDoF = 6 # Number of Degrees of freedom
# Solving iLQR
# ===============================
diff --git a/python/iLQR_bimanual.py b/python/iLQR_bimanual.py
index 7f75d7648508812f46c55f8031ed7b75b60242f5..a5979ebad1a525baf035580f0eb180556003b86d 100644
--- a/python/iLQR_bimanual.py
+++ b/python/iLQR_bimanual.py
@@ -27,7 +27,9 @@ import matplotlib.patches as patches
# Helper functions
# ===============================
-def fkin(x):
+
+# Forward kinematics for end-effector (in robot coordinate system)
+def fkin(param,x):
L = np.tril(np.ones(3))
x = x[np.newaxis, :] if x.ndim == 1 else x
f = np.vstack((param.l[:3] @ np.cos(L @ x[:, :3].T),
@@ -37,8 +39,8 @@ def fkin(x):
))
return f
-
-def fkin0(x):
+# Forward kinematics for all joints (in robot coordinate system)
+def fkin0(param,x):
L = np.tril(np.ones(3))
fl = np.vstack((L @ np.diag(param.l[:3]) @ np.cos(L @ x[:3].T),
L @ np.diag(param.l[:3]) @ np.sin(L @ x[:3].T)
@@ -51,18 +53,18 @@ def fkin0(x):
f = np.hstack((fl[:, ::-1], np.zeros((2, 1)), fr))
return f
-
-def f_reach(x):
- f = fkin(x) - param.Mu
+# Residual and Jacobian of end-effector for a viapoints reaching task (in robot coordinate system)
+def f_reach(param,x):
+ f = fkin(param,x) - param.Mu
f = f.ravel()
J = np.zeros((param.nbVarF * x.shape[0], x.shape[0] * param.nbVarX))
for t in range(x.shape[0]):
- Jtmp = Jkin(x[t])
+ Jtmp = Jkin(param,x[t])
J[t * param.nbVarF:(t + 1) * param.nbVarF, t * param.nbVarX:(t + 1) * param.nbVarX] = Jtmp
return f, J
-
-def Jkin(x):
+# Jacobian of the end-effector with analytical computation (for single time step)
+def Jkin(param,x):
L = np.tril(np.ones(3))
J = np.zeros((param.nbVarF, param.nbVarX))
Ju = np.vstack((-np.sin(L @ x[:3]) @ np.diag(param.l[:3]) @ L,
@@ -78,19 +80,19 @@ def Jkin(x):
J[2:, (0, 3, 4)] = Jl
return J
-
-def f_reach_CoM(x):
- f = fkin_CoM(x) - np.array([param.MuCoM]).T
+# Residual and Jacobian of Center of Mass for a viapoints reaching task (in robot coordinate system)
+def f_reach_CoM(param,x):
+ f = fkin_CoM(param,x) - np.array([param.MuCoM]).T
f = f.ravel(order="F")
J = np.zeros((2 * x.shape[0], x.shape[0] * param.nbVarX))
for t in range(x.shape[0]):
- Jtmp = Jkin_CoM(x[t])
+ Jtmp = Jkin_CoM(param,x[t])
J[t * 2:(t + 1) * 2, t * param.nbVarX:(t + 1) * param.nbVarX] = Jtmp
return f, J
-
-def fkin_CoM(x):
+# Forward kinematics of the center of mass
+def fkin_CoM(param,x):
L = np.tril(np.ones(3))
f = np.vstack((param.l[:3] @ L @ np.cos(L @ x[:, :3].T) +
np.array(param.l)[[0, 3, 4]] @ L @ np.cos(L @ x[:, (0, 3, 4)].T),
@@ -99,8 +101,8 @@ def fkin_CoM(x):
)) / 6
return f
-
-def Jkin_CoM(x):
+# Jacobian of the center of mass with analytical computation (for single time step)
+def Jkin_CoM(param,x):
L = np.tril(np.ones(3))
Jl = np.vstack((-np.sin(L @ x[:3]) @ L @ np.diag(param.l[:3] @ L),
np.cos(L @ x[:3]) @ L @ np.diag(param.l[:3] @ L)
@@ -156,8 +158,8 @@ Su = Su0[idx.flatten()] # We remove the lines that are out of interest
for i in range(param.nbIter):
x = Su0 @ u + Sx0 @ x0
x = x.reshape((param.nbData, param.nbVarX))
- f, J = f_reach(x[tl]) # Forward kinematics and Jacobian for end-effectors
- fc, Jc = f_reach_CoM(x) # Forward kinematics and Jacobian for center of mass
+ f, J = f_reach(param,x[tl]) # Forward kinematics and Jacobian for end-effectors
+ fc, Jc = f_reach_CoM(param,x) # Forward kinematics and Jacobian for center of mass
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 - Su0.T @ Jc.T @ Qc @ fc - u * param.r)
@@ -169,8 +171,8 @@ for i in range(param.nbIter):
while True:
utmp = u + du * alpha
xtmp = (Su0 @ utmp + Sx0 @ x0).reshape((param.nbData, param.nbVarX))
- ftmp, _ = f_reach(xtmp[tl])
- fctmp, _ = f_reach_CoM(xtmp)
+ ftmp, _ = f_reach(param,xtmp[tl])
+ fctmp, _ = f_reach_CoM(param,xtmp)
# for end-effectors and CoM
cost = ftmp.T @ Q @ ftmp + fctmp.T @ Qc @ fctmp + np.linalg.norm(utmp) ** 2 * param.r # for end-effectors and CoM
@@ -192,14 +194,14 @@ plt.axis("off")
plt.gca().set_aspect('equal', adjustable='box')
# Plot bimanual robot
-ftmp = fkin0(x[0])
+ftmp = fkin0(param,x[0])
plt.plot(ftmp[0], ftmp[1], c='black', linewidth=4, alpha=.2)
-ftmp = fkin0(x[-1])
+ftmp = fkin0(param,x[-1])
plt.plot(ftmp[0], ftmp[1], c='black', linewidth=4, alpha=.6)
# Plot CoM
-fc = fkin_CoM(x) # Forward kinematics for center of mass
+fc = fkin_CoM(param,x) # Forward kinematics for center of mass
plt.plot(fc[0, 0], fc[1, 0], c='black', marker='o', linewidth=0,
markersize=np.sqrt(90), markerfacecolor='none', markeredgewidth=3, alpha=.4) # Plot CoM
plt.plot(fc[0, -1], fc[1, -1], c='black', marker='o', linewidth=0,
@@ -214,7 +216,7 @@ plt.plot(param.MuCoM[0], param.MuCoM[1], c='red', marker='o', linewidth=0,
markersize=np.sqrt(90), markerfacecolor='none', markeredgewidth=2, alpha=.8)
# Plot end-effectors paths
-ftmp = fkin(x)
+ftmp = fkin(param,x)
plt.plot(ftmp[0, :], ftmp[1, :], c="black", marker="o", markevery=[0] + tl.tolist())
plt.plot(ftmp[2, :], ftmp[3, :], c="black", marker="o", markevery=[0] + tl.tolist())
plt.show()
diff --git a/python/iLQR_bimanual_manipulability.py b/python/iLQR_bimanual_manipulability.py
new file mode 100644
index 0000000000000000000000000000000000000000..ec0c8bb8f8c3206d3bba5ca7b1fe4802d9c18dc3
--- /dev/null
+++ b/python/iLQR_bimanual_manipulability.py
@@ -0,0 +1,231 @@
+'''
+ iLQR applied to a planar bimanual robot problem with a cost on tracking a desired
+ manipulability ellipsoid at the center of mass (batch formulation)
+
+ Copyright (c) 2021 Idiap Research Institute, http://www.idiap.ch/
+ Written by Boyang Ti <https://www.idiap.ch/~bti/> and
+ 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/>.
+'''
+
+import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+import scipy.linalg
+from scipy.linalg import fractional_matrix_power
+
+import numpy.matlib
+# Helper functions
+# ===============================
+
+# Forward kinematics for end-effector (in robot coordinate system)
+def fkin(x, param):
+ L = np.tril(np.ones(3))
+ f = np.vstack((
+ param.l[0:3].T @ np.cos(L @ x[0:3]),
+ param.l[0:3].T @ np.sin(L @ x[0:3]),
+ param.l[[0, 3, 4]].T @ np.cos(L @ x[[0, 3, 4]]),
+ param.l[[0, 3, 4]].T @ np.sin(L @ x[[0, 3, 4]])
+ )) # f1,f2
+ return f
+
+# Forward kinematics for end-effector (in robot coordinate system)
+def fkin0(x, param):
+ L = np.tril(np.ones(3))
+ fl = np.vstack((
+ L @ np.diag(param.l[0:3].flatten()) @ np.cos(L @ x[0:3]),
+ L @ np.diag(param.l[0:3].flatten()) @ np.sin(L @ x[0:3])
+ ))
+ fr = np.vstack((
+ L @ np.diag(param.l[[0, 3, 4]].flatten()) @ np.cos(L @ x[[0, 3, 4]]),
+ L @ np.diag(param.l[[0, 3, 4]].flatten()) @ np.sin(L @ x[[0, 3, 4]])
+ ))
+ f = np.hstack((fl[:, ::-1], np.zeros([2, 1]), fr))
+ return f
+
+# Forward kinematics for center of mass of a bimanual robot (in robot coordinate system)
+def fkin_CoM(x, param):
+ L = np.tril(np.ones(3))
+ f = np.vstack((param.l[0:3].T @ L @ np.cos(L @ x[0:3]) + param.l[[0, 3, 4]].T @ L @ np.cos(L @ x[[0, 3, 4]]),
+ param.l[0:3].T @ L @ np.sin(L @ x[0:3]) + param.l[[0, 3, 4]].T @ L @ np.sin(L @ x[[0, 3, 4]]))) / 6
+
+ 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,
+ np.ones([1, param.nbVarX])
+ ])
+ return J
+
+# Jacobian for center of mass of a robot (in robot coordinate system)
+def Jkin_CoM(x, param):
+ L = np.tril(np.ones(3))
+ Jl = np.vstack((-np.sin(L @ x[0:3].reshape(-1, 1)).T @ L @ np.diag((param.l[0:3].T @ L).flatten()),
+ np.cos(L @ x[0:3].reshape(-1, 1)).T @ L @ np.diag((param.l[0:3].T @ L).flatten()))) / 6
+ Jr = np.vstack((-np.sin(L @ x[[0, 3, 4]].reshape(-1, 1)).T @ L @ np.diag((param.l[[0, 3, 4]].T @ L).flatten()),
+ np.cos(L @ x[[0, 3, 4]].reshape(-1, 1)).T @ L @ np.diag((param.l[[0, 3, 4]].T @ L).flatten()))) / 6
+ J = np.hstack(((Jl[:, 0] + Jr[:, 0]).reshape(-1, 1), Jl[:, 1:], Jr[:, 1:]))
+ return J
+
+def rman(x, param):
+ G = fractional_matrix_power(param.MuS, -0.5)
+ f = np.zeros((3, np.size(x, 1)))
+ for i in range(np.size(x, 1)):
+ Jt = Jkin_CoM(x[:, i], param) # Jacobian for center of mass
+ St = Jt @ Jt.T # manipulability matrix
+
+ D, V = np.linalg.eig(G @ St @ G)
+ E = V @ np.diag(np.log(D)) @ np.linalg.pinv(V)
+
+ E = np.tril(E) * (np.eye(2) + np.tril(np.ones(2), -1) * np.sqrt(2))
+ f[:, i] = E[np.where(E!=0)]
+ return f
+
+# Jacobian for manipulability tracking with numerical computation
+def Jman_num(x, param):
+ e = 1E-6
+ X = np.matlib.repmat(x, 1, param.nbVarX)
+ F1 = rman(X, param)
+ F2 = rman(X + np.eye(param.nbVarX) * e, param)
+ J = (F2 - F1) / e
+ return J
+
+# Residuals f and Jacobians J for manipulability tracking
+# (c=f'*f is the cost, g=J'*f is the gradient, H=J'*J is the approximated Hessian)
+def f_manipulability(x, param):
+ f = rman(x, param) # Residuals
+ for t in range(np.size(x, 1)):
+ if t == 0:
+ J = Jman_num(x[:, t].reshape(-1, 1), param)
+ else:
+ J = scipy.linalg.block_diag(J, Jman_num(x[:, t].reshape(-1, 1), param)) # Jacobians
+ return f, J
+
+## Parameters
+param = lambda: None # Lazy way to define an empty class in python
+
+param.dt = 1e0 # Time step length
+param.nbIter = 100 # Maximum number of iterations for iLQR
+param.nbPoints = 1 # Number of viapoints
+param.nbData = 10 # Number of datapoints
+param.nbVarX = 5 # State space dimension ([q1,q2,q3] for left arm, [q1,q4,q5] for right arm)
+param.nbVarU = param.nbVarX # Control space dimension ([dq1, dq2, dq3, dq4, dq5])
+param.nbVarF = 4 # Objective function dimension ([x1,x2] for left arm and [x3,x4] for right arm)
+param.l = np.ones((param.nbVarX, 1)) * 2. # Robot links lengths
+param.r = 1e-6 # Control weight term
+param.MuS = np.asarray([[10, 2], [2, 4]])
+
+# Precision matrix
+Q = np.kron(np.identity(param.nbPoints), np.diag([0., 0., 0., 0.]))
+# Control weight matrix
+R = np.identity((param.nbData-1) * param.nbVarU) * param.r
+# Precision matrix for continuous CoM tracking
+Qc = np.kron(np.identity(param.nbData), np.diag([0., 0.]))
+
+# 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)])
+
+# iLQR
+# ===============================
+u = np.zeros(param.nbVarU * (param.nbData-1)) # Initial control command
+x0 = np.array([np.pi/3, np.pi/2, np.pi/3, -np.pi/3, -np.pi/4]) # Initial state
+
+# 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))
+])
+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
+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_manipulability(x[:, tl], param) # Residuals and Jacobians
+ du = np.linalg.inv(Su.T @ J.T @ J @ Su + R) @ (-Su.T @ J.T @ f.flatten('F') - u * param.r) # Gauss-Newton update
+ # Estimate step size with backtracking line search method
+ alpha = 1
+ cost0 = f.flatten('F').T @ f.flatten('F') + np.linalg.norm(u)**2 * param.r # Cost
+ while True:
+ utmp = u + du * alpha
+ xtmp = Su0 @ utmp + Sx0 @ x0 # System evolution
+ xtmp = xtmp.reshape([param.nbVarX, param.nbData], order='F')
+ ftmp, _ = f_manipulability(xtmp[:, tl], param) # Residuals
+ cost = ftmp.flatten('F').T @ ftmp.flatten('F') + np.linalg.norm(utmp)**2 * param.r # Cost
+ if cost < cost0 or alpha < 1e-3:
+ u = utmp
+ print("Iteration {}, cost: {}".format(i,cost))
+ break
+ alpha /= 2
+ if np.linalg.norm(du * alpha) < 1E-2:
+ break # Stop iLQR iterations when solution is reached
+
+# Plots
+# ===============================
+plt.figure()
+plt.axis('off')
+plt.gca().set_aspect('equal', adjustable='box')
+
+fc = fkin_CoM(x, param)
+al = np.linspace(-np.pi, np.pi, 50)
+ax = plt.gca()
+
+# Plot desired manipulability ellipsoid
+D1, V1 = np.linalg.eig(param.MuS)
+D1 = np.diag(D1)
+R1 = np.real(V1@np.sqrt(D1+0j))
+msh1 = (R1 @ np.array([np.cos(al), np.sin(al)]) * 0.52).T + np.matlib.repmat(fc[:, -1].reshape(-1, 1), 1, 50).T
+p1 = patches.Polygon(msh1, closed=True, alpha=0.9)
+p1.set_facecolor([1, 0.7, 0.7])
+p1.set_edgecolor([1, 0.6, 0.6])
+ax.add_patch(p1)
+
+# Plot robot manipulability ellipsoid
+J = Jkin_CoM(x[:, -1], param)
+S = J @ J.T
+D2, V2 = np.linalg.eig(S)
+D2 = np.diag(D2)
+R2 = np.real(V2@np.sqrt(D2+0j))
+msh2 = (R2 @ np.array([np.cos(al), np.sin(al)]) * 0.5).T + np.matlib.repmat(fc[:, -1].reshape(-1, 1), 1, 50).T
+p2 = patches.Polygon(msh2, closed=True, alpha=0.9)
+p2.set_facecolor([0.4, 0.4, 0.4])
+p2.set_edgecolor([0.3, 0.3, 0.3])
+ax.add_patch(p2)
+
+# Plot CoM
+fc = fkin_CoM(x, param) # Forward kinematics for center of mass
+plt.plot(fc[0, 0], fc[1, 0], marker='o', markerfacecolor='none', markeredgewidth=4, markersize=10, markeredgecolor=[0.5, 0.5, 0.5]) # Plot CoM
+plt.plot(fc[0, tl[-1]], fc[1, tl[-1]], marker='o', markerfacecolor='none', markeredgewidth=4, markersize=10, markeredgecolor=[0.2, 0.2, 0.2]) # Plot CoM
+
+# Plot end-effectors paths
+f01 = fkin0(x[:, 0], param)
+f02 = fkin0(x[:, tl[0]], param)
+
+# Get points of interest
+f = fkin(x, param)
+plt.plot(f01[0, :], f01[1, :], c='black', linewidth=4, alpha=.2)
+plt.plot(f02[0, :], f02[1, :], c='black', linewidth=4, alpha=.4)
+
+plt.plot(f[0, :], f[1, :], c='black', marker='o', markevery=[0]+tl.tolist())
+plt.plot(f[2, :], f[3, :], c='black', marker='o', markevery=[0]+tl.tolist())
+
+
+plt.show()
diff --git a/python/iLQR_car.py b/python/iLQR_car.py
index 89231114c453f21488c3129f8ba811588fa2b8a4..6d74c7943d7079bdd2c56b509048150c0655d679 100644
--- a/python/iLQR_car.py
+++ b/python/iLQR_car.py
@@ -26,14 +26,14 @@ import matplotlib.pyplot as plt
# Parameters
# ===============================
param = lambda: None # Lazy way to define an empty class in python
-param.car_length=0.2
-param.dt=1e-2
-param.x_d=np.array([2, 1, np.pi])
-param.precision=1e3
-param.r=1e-3
-param.horizon=1.0
-param.nbIter=30
-param.nbSteps = int(param.horizon / param.dt)
+param.car_length=0.2 # Size of the car
+param.dt=1e-2 # delta t
+param.x_d=np.array([2, 1, np.pi]) # Desired position
+param.precision=1e3 # Precision weight term
+param.r=1e-3 # Control weight term
+param.horizon=1.0 # Duration of the control
+param.nbIter=30 # Number of iLQR iteration
+param.nbSteps = int(param.horizon / param.dt) # Number of discrete timesteps
param.nbDoF = 5 # set 4 for velocity control, 5 for acceleration control
##### Velocity Control functions
diff --git a/python/iLQR_manipulator.py b/python/iLQR_manipulator.py
index 9f4d7a5a6668e32e5d667c1a6e70a0d11893aa3f..2aa963d47157c32745eb03efae5692997e361aa9 100644
--- a/python/iLQR_manipulator.py
+++ b/python/iLQR_manipulator.py
@@ -44,7 +44,7 @@ def fkin(x, param):
]) # f1,f2,f3, where f3 is the orientation (single Euler angle for planar robot)
return f
-# Forward kinematics for end-effector (in robot coordinate system)
+# 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([
@@ -64,12 +64,12 @@ def Jkin(x, param):
])
return J
-# Error and Jacobian for a viapoints reaching task (in object coordinate system)
+# Residual and Jacobian for a viapoints reaching task (in object coordinate system)
def f_reach(x, param):
f = logmap(fkin(x, param), param.Mu)
J = np.zeros([param.nbPoints * param.nbVarF, param.nbPoints * param.nbVarX])
for t in range(param.nbPoints):
- f[:2,t] = param.A[:,:,t].T @ f[:2,t] # Object oriented forward kinematics
+ f[:2,t] = param.A[:,:,t].T @ f[:2,t] # Object oriented residual
Jtmp = Jkin(x[:,t], param)
Jtmp[:2] = param.A[:,:,t].T @ Jtmp[:2] # Object centered Jacobian
@@ -85,6 +85,8 @@ def f_reach(x, param):
return f,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
@@ -104,8 +106,6 @@ param.useBoundingBox = True # Consider bounding boxes for reaching cost
# Main program
# ===============================
-#param = Param();
-
# Object rotation matrices
for t in range(param.nbPoints):
orn_t = param.Mu[-1,t]
@@ -184,12 +184,10 @@ plt.plot(f[0,:], f[1,:], c='black', marker='o', markevery=[0]+tl.tolist())
ax = plt.gca()
color_map = ['deepskyblue','darkorange']
for t in range(param.nbPoints):
- if param.useBoundingBox:
- rect_origin = param.Mu[:2,t] - param.A[:,:,t] @ np.array(param.sz)
- rect_orn = param.Mu[-1,t]
- rect = patches.Rectangle(rect_origin, param.sz[0]*2, param.sz[1]*2, np.degrees(rect_orn), color=color_map[t])
- ax.add_patch(rect)
- else:
- plt.scatter(param.Mu[0,t], param.Mu[1,t], s=100, marker='X', c=color_map[t])
+ rect_origin = param.Mu[:2,t] - param.A[:,:,t] @ np.array(param.sz)
+ rect_orn = param.Mu[-1,t]
+ rect = patches.Rectangle(rect_origin, param.sz[0]*2, param.sz[1]*2, np.degrees(rect_orn), color=color_map[t])
+ ax.add_patch(rect)
+ #plt.scatter(param.Mu[0,t], param.Mu[1,t], s=100, marker='X', c=color_map[t])
plt.show()
diff --git a/python/iLQR_manipulator_CP.py b/python/iLQR_manipulator_CP.py
index 3e77cf8abfec51d0c90bf2b45cf9979ab15d944c..f92a7dd3c1ae47d1989b608ce8111d705e68d100 100644
--- a/python/iLQR_manipulator_CP.py
+++ b/python/iLQR_manipulator_CP.py
@@ -38,8 +38,7 @@ def logmap(f,f0):
return error
# Forward kinematics for E-E
-def fkin(x):
- global param
+def fkin(param,x):
x = x.T
A = np.tril(np.ones([param.nbVarX,param.nbVarX]))
f = np.vstack((param.linkLengths @ np.cos(A @ x),
@@ -48,7 +47,7 @@ def fkin(x):
return f.T
# Forward Kinematics for all joints
-def fkin0(x):
+def fkin0(param,x):
T = np.tril(np.ones([param.nbVarX,param.nbVarX]))
T2 = np.tril(np.matlib.repmat(param.linkLengths,len(x),1))
f = np.vstack((
@@ -61,8 +60,8 @@ def fkin0(x):
))
return f
-def jkin(x):
- global param
+# 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.linkLengths) @ T,
@@ -71,16 +70,14 @@ def jkin(x):
))
return J
-# Cost and gradient
-def f_reach( x ):
- global param
-
- f = logmap(fkin(x),param.mu)
+# Residual and Jacobian
+def f_reach(param,x):
+ f = logmap(fkin(param,x),param.mu)
J = np.zeros(( len(x) * param.nbVarF , len(x) * param.nbVarX ))
for t in range(x.shape[0]):
f[t,:2] = param.A[t].T @ f[t,:2] # Object oriented fk
- Jtmp = jkin(x[t])
+ Jtmp = jkin(param,x[t])
Jtmp[:2] = param.A[t].T @ Jtmp[:2] # Object centered jacobian
if param.useBoundingBox:
@@ -202,7 +199,7 @@ for i in range( param.nbIter ):
x = np.real(Su0 @ u + Sx0 @ x0)
x = x.reshape( ( param.nbData , param.nbVarX) )
- f, J = f_reach(x[tl])
+ f, J = f_reach(param,x[tl])
dw = np.linalg.inv(PSI.T @ Su.T @ J.T @ Q @ J @ Su @ PSI + PSI.T @ R @ PSI) @ (-PSI.T @ Su.T @ J.T @ Q @ f.flatten() - PSI.T @ u * param.r)
du = PSI @ dw
# Perform line search
@@ -213,7 +210,7 @@ for i in range( param.nbIter ):
utmp = u + du * alpha
xtmp = np.real(Su0 @ utmp + Sx0 @ x0)
xtmp = xtmp.reshape( ( param.nbData , param.nbVarX) )
- ftmp, _ = f_reach(xtmp[tl])
+ ftmp, _ = f_reach(param,xtmp[tl])
cost = ftmp.flatten() @ Q @ ftmp.flatten() + np.linalg.norm(utmp) * param.r
if cost < cost0 or alpha < 1e-3:
@@ -234,10 +231,10 @@ plt.axis("off")
plt.gca().set_aspect('equal', adjustable='box')
# Get points of interest
-f = fkin(x)
-f00 = fkin0(x[0])
-f10 = fkin0(x[tl[0]])
-fT0 = fkin0(x[-1])
+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))
plt.plot( f00[:,0] , f00[:,1],c='black',linewidth=5,alpha=.2)
diff --git a/python/iLQR_manipulator_CoM.py b/python/iLQR_manipulator_CoM.py
index 3c9debe518f2d743b34570859ba0bfa30cadfa7a..6f74266847d4381d6aab25d8b64af172a8741664 100644
--- a/python/iLQR_manipulator_CoM.py
+++ b/python/iLQR_manipulator_CoM.py
@@ -27,9 +27,9 @@ import matplotlib.patches as patches
# Helper functions
# ===============================
+
# Forward kinematics (in robot coordinate system)
-def fkin(x):
- global param
+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),
@@ -37,7 +37,7 @@ def fkin(x):
return f
# Forward Kinematics for all joints
-def fkin0(x):
+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((
@@ -51,8 +51,7 @@ def fkin0(x):
return f
# Jacobian with analytical computation (for single time step)
-def jkin(x):
- global param
+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,
@@ -60,31 +59,27 @@ def jkin(x):
))
return J
-# Cost and gradient for end-effector
-def f_reach(x):
- global param
-
- f = fkin(x).T - param.Mu
+# 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(x[t])
+ 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
# Forward kinematics for center of mass (in robot coordinate system, with mass located at the joints)
-def fkin_CoM(x):
- global param
+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
return f
-# Cost and gradient for center of mass
-def f_reach_CoM(x):
- global param
- f = fkin_CoM(x).T - param.MuCoM
+# 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]):
@@ -112,11 +107,11 @@ param.nbVarX = 5 # State space dimension (x1,x2,x3)
param.nbVarU = 5 # Control space dimension (dx1,dx2,dx3)
param.nbVarF = 2 # Objective function dimension (f1,f2,f3, with f3 as orientation)
param.l = [2, 2, 2, 2, 2] # Robot links lengths
-param.szCoM = .6
+param.szCoM = .6 # Size of the center of mass
param.useBoundingBox = True
param.r = 1e-5 # Control weight term
param.Mu = np.asarray([3.5, 4]) # Target
-param.MuCoM = np.asarray([.4, 0])
+param.MuCoM = np.asarray([.4, 0]) # desired position of the center of mass
# Main program
# ===============================
@@ -156,8 +151,8 @@ for i in range(param.nbIter):
x = Su0 @ u + Sx0 @ x0
x = x.reshape((param.nbData, param.nbVarX))
- f, J = f_reach(x[tl])
- fc, Jc = f_reach_CoM (x)
+ 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)
# Perform line search
@@ -168,8 +163,8 @@ for i in range(param.nbIter):
utmp = u + du * alpha
xtmp = Su0 @ utmp + Sx0 @ x0
xtmp = xtmp.reshape((param.nbData, param.nbVarX))
- ftmp, _ = f_reach(xtmp[tl])
- fctmp, _ = f_reach_CoM(xtmp)
+ 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
if cost < cost0 or alpha < 1e-3:
@@ -189,9 +184,9 @@ 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(x[0])
-fT0 = fkin0(x[-1])
-fc = fkin_CoM(x)
+f00 = fkin0(param,x[0])
+fT0 = fkin0(param,x[-1])
+fc = fkin_CoM(param,x)
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='-')
diff --git a/python/iLQR_manipulator_dynamics.py b/python/iLQR_manipulator_dynamics.py
index bd580511c73dd30e0bcd901abffcd1639fbf3de7..471e2f3e018106fa36eb8a4e4c4b3d620303e5d3 100644
--- a/python/iLQR_manipulator_dynamics.py
+++ b/python/iLQR_manipulator_dynamics.py
@@ -21,7 +21,6 @@
'''
import numpy as np
-import numpy.matlib
import matplotlib.pyplot as plt
import matplotlib.patches as patches
@@ -36,49 +35,43 @@ def logmap(f,f0):
return error
# Forward kinematics for E-E
-def fkin(x):
- global param
- x = x.T
- A = np.tril(np.ones([param.nbVarX,param.nbVarX]))
- f = np.vstack((param.linkLengths @ np.cos(A @ x),
- param.linkLengths @ np.sin(A @ x),
- np.mod(np.sum(x,0)+np.pi, 2*np.pi) - np.pi)) #x1,x2,o (orientation as single Euler angle for planar robot)
- return f.T
+def fkin(x,param):
+ L = np.tril(np.ones([param.nbVarX, param.nbVarX]))
+ f = np.vstack([
+ param.l @ np.cos(L @ x.T),
+ param.l @ np.sin(L @ x.T),
+ np.mod(np.sum(x.T,0)+np.pi, 2*np.pi) - np.pi
+ ]) # f1,f2,f3, where f3 is the orientation (single Euler angle for planar robot)
+ return f.T
# Forward Kinematics for all joints
-def fkin0(x):
- T = np.tril(np.ones([param.nbVarX,param.nbVarX]))
- T2 = np.tril(np.matlib.repmat(param.linkLengths,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
-
-def jkin(x):
- global param
- T = np.tril( np.ones((len(x),len(x))) )
+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.T
+
+# Jacobian with analytical computation (for single time step)
+def Jkin(x,param):
+ L = np.tril( np.ones((len(x),len(x))) )
J = np.vstack((
- -np.sin(T@x).T @ np.diag(param.linkLengths) @ T,
- np.cos(T@x).T @ np.diag(param.linkLengths) @ T,
+ -np.sin(L@x).T @ np.diag(param.l) @ L,
+ np.cos(L@x).T @ np.diag(param.l) @ L,
np.ones(len(x))
))
return J
-# Cost and gradient
-def f_reach( x ):
- global param
+# Residual and Jacobian
+def f_reach(x,param):
+ f = logmap(fkin(x,param),param.Mu)
+ J = np.zeros(( param.nbPoints * param.nbVarF , param.nbPoints * param.nbVarX ))
- f = logmap(fkin(x),param.mu)
- J = np.zeros(( len(x) * param.nbVarF , len(x) * param.nbVarX ))
-
- for t in range(x.shape[0]):
+ for t in range(param.nbPoints ):
f[t,:2] = param.A[t].T @ f[t,:2] # Object oriented fk
- Jtmp = jkin(x[t])
+ Jtmp = Jkin(x[t],param)
Jtmp[:2] = param.A[t].T @ Jtmp[:2] # Object centered jacobian
if param.useBoundingBox:
@@ -94,74 +87,69 @@ def f_reach( x ):
# Forward dynamic to compute
def forward_dynamics(x, u, param):
- g = 9.81 #Gravity norm
- kv = 1 #Joints Damping
- l = np.reshape( param.linkLengths, [1,param.nbVarX] )
+ l = np.reshape( param.l, [1,param.nbVarX] )
m = np.reshape( param.linkMasses, [1,param.nbVarX] )
- dt = param.dt
- nbDOFs = l.shape[1]
- nbData = int(u.shape[0]/nbDOFs + 1)
- Tm = np.multiply(np.triu(np.ones([nbDOFs,nbDOFs])), np.repeat(m, nbDOFs,0))
- T = np.tril(np.ones([nbDOFs, nbDOFs]))
- Su = np.zeros([2 * nbDOFs * nbData, nbDOFs * (nbData - 1)])
+ Tm = np.multiply(np.triu(np.ones([param.nbVarX,param.nbVarX])), np.repeat(m, param.nbVarX,0))
+ T = np.tril(np.ones([param.nbVarX, param.nbVarX]))
+ Su = np.zeros([2 * param.nbVarX * param.nbData , param.nbVarX * (param.nbData - 1)])
#Precomputation of mask (in tensor form)
- S1= np.zeros([nbDOFs, nbDOFs, nbDOFs])
- J_index = np.ones([1, nbDOFs])
- for j in range(nbDOFs):
+ S1= np.zeros([param.nbVarX, param.nbVarX, param.nbVarX])
+ J_index = np.ones([1, param.nbVarX])
+ for j in range(param.nbVarX):
J_index[0,:j] = np.zeros([j])
- S1[:,:,j] = np.repeat(J_index @ np.eye(nbDOFs), nbDOFs, 0) - np.transpose(np.repeat(J_index @ np.eye(nbDOFs), nbDOFs, 0))
+ S1[:,:,j] = np.repeat(J_index @ np.eye(param.nbVarX), param.nbVarX, 0) - np.transpose(np.repeat(J_index @ np.eye(param.nbVarX), param.nbVarX, 0))
#Initialization of dM and dC tensors and A21 matrix
- dM = np.zeros([nbDOFs, nbDOFs, nbDOFs])
- dC = np.zeros([nbDOFs, nbDOFs, nbDOFs])
- A21 = np.zeros([nbDOFs, nbDOFs])
+ dM = np.zeros([param.nbVarX, param.nbVarX, param.nbVarX])
+ dC = np.zeros([param.nbVarX, param.nbVarX, param.nbVarX])
+ A21 = np.zeros([param.nbVarX, param.nbVarX])
- for t in range(nbData-1):
+ for t in range(param.nbData -1):
# Computation in matrix form of G,M, and C
- G =-np.reshape(np.sum(Tm,1), [nbDOFs,1]) * l.T * np.cos(T @ np.reshape(x[t,0:nbDOFs], [nbDOFs,1])) * g
+ G =-np.reshape(np.sum(Tm,1), [param.nbVarX,1]) * l.T * np.cos(T @ np.reshape(x[t,0:param.nbVarX], [param.nbVarX,1])) * param.g
G = T.T @ G
- M = (l.T * l) * np.cos(np.reshape(T @ x[t,:nbDOFs], [nbDOFs,1]) - T @ x[t,:nbDOFs]) * (Tm ** .5 @ ((Tm ** .5).T))
+ M = (l.T * l) * np.cos(np.reshape(T @ x[t,:param.nbVarX], [param.nbVarX,1]) - T @ x[t,:param.nbVarX]) * (Tm ** .5 @ ((Tm ** .5).T))
M = T.T @ M @ T
- C = -(l.T * l) * np.sin(np.reshape(T @ x[t,:nbDOFs], [nbDOFs,1]) - T @ x[t,:nbDOFs]) * (Tm ** .5 @ ((Tm ** .5).T))
+ C = -(l.T * l) * np.sin(np.reshape(T @ x[t,:param.nbVarX], [param.nbVarX,1]) - T @ x[t,:param.nbVarX]) * (Tm ** .5 @ ((Tm ** .5).T))
# Computation in tensor form of derivatives dG,dM, and dC
- dG = np.diagflat(np.reshape(np.sum(Tm,1), [nbDOFs,1]) * l.T * np.sin(T @ np.reshape(x[t,0:nbDOFs], [nbDOFs,1])) * g) @ T
- dM_tmp = (l.T * l) * np.sin(np.reshape(T @ x[t,:nbDOFs], [nbDOFs,1]) - T @ x[t,:nbDOFs]) * (Tm ** .5 @ ((Tm ** .5).T))
+ dG = np.diagflat(np.reshape(np.sum(Tm,1), [param.nbVarX,1]) * l.T * np.sin(T @ np.reshape(x[t,0:param.nbVarX], [param.nbVarX,1])) * param.g) @ T
+ dM_tmp = (l.T * l) * np.sin(np.reshape(T @ x[t,:param.nbVarX], [param.nbVarX,1]) - T @ x[t,:param.nbVarX]) * (Tm ** .5 @ ((Tm ** .5).T))
for j in range(dM.shape[2]):
dM[:,:,j] = T.T @ (dM_tmp * S1[:,:,j]) @ T
- dC_tmp = (l.T * l) * np.cos(np.reshape( T @ x[t,:nbDOFs], [nbDOFs,1]) - T @ x[t,:nbDOFs]) * (Tm ** .5 @ ((Tm ** .5).T))
+ dC_tmp = (l.T * l) * np.cos(np.reshape( T @ x[t,:param.nbVarX], [param.nbVarX,1]) - T @ x[t,:param.nbVarX]) * (Tm ** .5 @ ((Tm ** .5).T))
for j in range(dC.shape[2]):
dC[:,:,j] = dC_tmp * S1[:,:,j]
# update pose
- tau = np.reshape(u[(t) * nbDOFs:(t + 1) * nbDOFs], [nbDOFs, 1])
+ tau = np.reshape(u[(t) * param.nbVarX:(t + 1) * param.nbVarX], [param.nbVarX, 1])
inv_M = np.linalg.inv(M)
- ddq =inv_M @ (tau + G + T.T @ C @ (T @ np.reshape(x[t,nbDOFs:], [nbDOFs,1])) ** 2) - T @ np.reshape(x[t,nbDOFs:], [nbDOFs,1]) * kv
+ ddq =inv_M @ (tau + G + T.T @ C @ (T @ np.reshape(x[t,param.nbVarX:], [param.nbVarX,1])) ** 2) - T @ np.reshape(x[t,param.nbVarX:], [param.nbVarX,1]) * param.kv
# compute local linear systems
- x[t+1,:] = x[t,:] + np.hstack([x[t,nbDOFs:], np.reshape(ddq, [nbDOFs,])]) * dt
- A11 = np.eye(nbDOFs)
- A12 = A11 * dt
- A22 = np.eye(nbDOFs) + (2 * inv_M @ T.T @ C @ np.diagflat(T @ x[t,nbDOFs:]) @ T - T * kv) * dt
- for j in range(nbDOFs):
- A21[:,j] = (-inv_M @ dM[:,:,j] @ inv_M @ (tau + G + T.T @ C @ (T @ np.reshape(x[t,nbDOFs:], [nbDOFs,1])) ** 2)
- + np.reshape(inv_M @ T.T @ dG[:,j], [nbDOFs,1]) + inv_M @ T .T @ dC[:,:,j] @ (T @ np.reshape(x[t,nbDOFs:], [nbDOFs,1])) ** 2).flatten()
- A = np.vstack((np.hstack((A11, A12)), np.hstack((A21 * dt, A22))))
- B = np.vstack((np.zeros([nbDOFs, nbDOFs]), inv_M * dt))
+ x[t+1,:] = x[t,:] + np.hstack([x[t,param.nbVarX:], np.reshape(ddq, [param.nbVarX,])]) * param.dt
+ A11 = np.eye(param.nbVarX)
+ A12 = A11 * param.dt
+ A22 = np.eye(param.nbVarX) + (2 * inv_M @ T.T @ C @ np.diagflat(T @ x[t,param.nbVarX:]) @ T - T * param.kv) * param.dt
+ for j in range(param.nbVarX):
+ A21[:,j] = (-inv_M @ dM[:,:,j] @ inv_M @ (tau + G + T.T @ C @ (T @ np.reshape(x[t,param.nbVarX:], [param.nbVarX,1])) ** 2)
+ + np.reshape(inv_M @ T.T @ dG[:,j], [param.nbVarX,1]) + inv_M @ T .T @ dC[:,:,j] @ (T @ np.reshape(x[t,param.nbVarX:], [param.nbVarX,1])) ** 2).flatten()
+ A = np.vstack((np.hstack((A11, A12)), np.hstack((A21 * param.dt, A22))))
+ B = np.vstack((np.zeros([param.nbVarX, param.nbVarX]), inv_M * param.dt))
# compute transformation matrix
- Su[2 * nbDOFs * (t + 1):2 * nbDOFs * (t + 2),:] = A @ Su[2 * nbDOFs * t:2 * nbDOFs * (t + 1),:]
- Su[2 * nbDOFs * (t + 1):2 * nbDOFs * (t + 2), nbDOFs * t:nbDOFs * (t + 1)] =B
+ Su[2 * param.nbVarX * (t + 1):2 * param.nbVarX * (t + 2),:] = A @ Su[2 * param.nbVarX * t:2 * param.nbVarX * (t + 1),:]
+ Su[2 * param.nbVarX * (t + 1):2 * param.nbVarX * (t + 2), param.nbVarX * t:param.nbVarX * (t + 1)] =B
return x, Su
-# General param parameters
+# Parameters
# ===============================
param = lambda: None # Lazy way to define an empty class in python
@@ -172,66 +160,54 @@ param.nbPoints = 2 # Number of viapoints
param.nbVarX = 3 # State space dimension (x1,x2,x3)
param.nbVarU = 3 # Control space dimension (dx1,dx2,dx3)
param.nbVarF = 3 # Objective function dimension (f1,f2,f3, with f3 as orientation)
-param.linkLengths = [2,2,1] # Robot links lengths
+param.l = [2,2,1] # Robot links lengths
param.linkMasses = [1,1,1]
-param.sizeObj = [.2,.3] # Size of objects
+param.g = 9.81 # Gravity norm
+param.kv = 1 # Joint damping
+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.Mu = np.asarray([[2,1,-np.pi/3], [3,2,-np.pi/3]]) # Viapoints
param.useBoundingBox = False
+param.A = np.zeros( (param.nbPoints,2,2) ) # Object orientation matrices
-# Task parameters
+# Main program
# ===============================
-# Targets
-param.mu = np.asarray([
- [2,1,-np.pi/3], # x , y , orientation
- [3,2,-np.pi/3]
-])
-
# Transformation matrices
-param.A = np.zeros( (param.nbPoints,2,2) )
for i in range(param.nbPoints):
- orn_t = param.mu[i,-1]
+ orn_t = param.Mu[i,-1]
param.A[i,:,:] = np.asarray([
[np.cos(orn_t) , -np.sin(orn_t)],
[np.sin(orn_t) , np.cos(orn_t)]
])
-# Regularization matrix
-R = np.identity( (param.nbData-1) * param.nbVarU ) * param.r
-
# Precision matrix
Q = np.identity( param.nbVarF * param.nbPoints)*1e5
-# System parameters
-# ===============================
+# Regularization matrix
+R = np.identity( (param.nbData-1) * param.nbVarU ) * param.r
# Time occurence 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* 2* param.nbVarX)])
+# iLQR
+# ===============================
+
u = np.zeros( param.nbVarU * (param.nbData-1) ) # Initial control command
x0 = np.array( [3 * np.pi/4 , -np.pi/2 , - np.pi/4] ) # Initial position (in joint space)
-v0 = np.array([0,0,0])#initial velocity (in joint space)
+v0 = np.array([0,0,0]) #initial velocity (in joint space)
x = np.zeros([param.nbData, 2*param.nbVarX])
x[0,:param.nbVarX] = x0
x[0,param.nbVarX:] = v0
-# 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))])
-Sx0 = np.kron( np.ones(param.nbData) , np.identity(param.nbVarX) ).T
- # We remove the lines that are out of interest
-
-# Solving iLQR
-# ===============================
for i in range( param.nbIter ):
# system evolution and Transfer matrix (computed from forward dynamics)
x, Su0 = forward_dynamics(x, u, param)
Su = Su0[idx.flatten()]
- f, J = f_reach(x[tl,:param.nbVarX])
+ f, J = f_reach(x[tl,:param.nbVarX],param)
du = np.linalg.inv(Su.T @ J.T @ Q @ J @ Su + R) @ (-Su.T @ J.T @ Q @ f.flatten() - u * param.r)
# Perform line search
@@ -241,7 +217,7 @@ for i in range( param.nbIter ):
while True:
utmp = u + du * alpha
xtmp, _ = forward_dynamics(x, utmp, param)
- ftmp, _ = f_reach(xtmp[tl,:param.nbVarX])
+ ftmp, _ = f_reach(xtmp[tl,:param.nbVarX],param)
cost = ftmp.flatten() @ Q @ ftmp.flatten() + np.linalg.norm(utmp) * param.r
if cost < cost0 or alpha < 1e-3:
@@ -261,9 +237,9 @@ plt.axis("off")
plt.gca().set_aspect('equal', adjustable='box')
# Get points of interest
-f = fkin(x[:,:param.nbVarX])
-f00 = fkin0(x[0,:param.nbVarX])
-fT0 = fkin0(x[-1,:param.nbVarX])
+f = fkin(x[:,:param.nbVarX],param)
+f00 = fkin0(x[0,:param.nbVarX],param)
+fT0 = fkin0(x[-1,:param.nbVarX],param)
plt.plot( f00[:,0] , f00[:,1],c='black',linewidth=5,alpha=.2)
plt.plot( fT0[:,0] , fT0[:,1],c='black',linewidth=5,alpha=.6)
@@ -276,12 +252,12 @@ color_map = ["deepskyblue","darkorange"]
for i in range(param.nbPoints):
if param.useBoundingBox:
- rect_origin = param.mu[i,:2] - param.A[i]@np.array(param.sizeObj)
- rect_orn = param.mu[i,-1]
+ rect_origin = param.Mu[i,:2] - param.A[i]@np.array(param.sizeObj)
+ rect_orn = param.Mu[i,-1]
rect = patches.Rectangle(rect_origin,param.sizeObj[0]*2,param.sizeObj[1]*2,np.degrees(rect_orn),color=color_map[i])
ax.add_patch(rect)
else:
- plt.scatter(param.mu[i,0],param.mu[i,1],s=100,marker="X",c=color_map[i])
+ plt.scatter(param.Mu[i,0],param.Mu[i,1],s=100,marker="X",c=color_map[i])
plt.show()
diff --git a/python/iLQR_manipulator_object_affordance.py b/python/iLQR_manipulator_object_affordance.py
index 3afc9cfc067085535eea94389603cef92cb5612a..3d846bb70511a9890c1dd19a3b83d3c6aa519157 100644
--- a/python/iLQR_manipulator_object_affordance.py
+++ b/python/iLQR_manipulator_object_affordance.py
@@ -37,8 +37,7 @@ def logmap(f,f0):
return error
# Forward kinematics for E-E
-def fkin(x):
- global param
+def fkin(param,x):
x = x.T
A = np.tril(np.ones([param.nbVarX,param.nbVarX]))
f = np.vstack((param.linkLengths @ np.cos(A @ x),
@@ -47,7 +46,7 @@ def fkin(x):
return f.T
# Forward Kinematics for all joints
-def fkin0(x):
+def fkin0(param,x):
T = np.tril(np.ones([param.nbVarX,param.nbVarX]))
T2 = np.tril(np.matlib.repmat(param.linkLengths,len(x),1))
f = np.vstack((
@@ -60,8 +59,8 @@ def fkin0(x):
))
return f
-def jkin(x):
- global param
+# 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.linkLengths) @ T,
@@ -70,16 +69,14 @@ def jkin(x):
))
return J
-# Cost and gradient
-def f_reach( x , bounding_boxes = True ):
- global param
-
- f = logmap(fkin(x),param.mu)
+# Residual and Jacobian
+def f_reach(param,x,bounding_boxes=True):
+ f = logmap(fkin(param,x),param.mu)
J = np.zeros(( len(x) * param.nbVarF , len(x) * param.nbVarX ))
for t in range(x.shape[0]):
f[t,:2] = param.A[t].T @ f[t,:2] # Object oriented fk
- Jtmp = jkin(x[t])
+ Jtmp = jkin(param,x[t])
Jtmp[:2] = param.A[t].T @ Jtmp[:2] # Object centered jacobian
if bounding_boxes:
@@ -108,6 +105,7 @@ 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.tracking_term = 1
+
# Task parameters
# ===============================
@@ -161,8 +159,8 @@ for i in range( param.nbIter ):
x = Su0 @ u + Sx0 @ x0
x = x.reshape( ( param.nbData , param.nbVarX) )
- f, J = f_reach(x[tl])
- fc , Jc = f_reach(x[tl],False)
+ f, J = f_reach(param,x[tl])
+ fc , Jc = f_reach(param,x[tl],False)
du = np.linalg.inv(Su.T @ J.T @ Q @ J @ Su + Su.T @ Jc.T @ Qc @ Jc @ Su + R) @ (-Su.T @ J.T @ Q @ f.flatten() - Su.T @ Jc.T @ Qc @ fc.flatten() - u * param.r)
# Perform line search
@@ -173,8 +171,8 @@ for i in range( param.nbIter ):
utmp = u + du * alpha
xtmp = Su0 @ utmp + Sx0 @ x0
xtmp = xtmp.reshape( ( param.nbData , param.nbVarX) )
- ftmp, _ = f_reach(xtmp[tl])
- fctmp, _ = f_reach(xtmp[tl],False)
+ ftmp, _ = f_reach(param,xtmp[tl])
+ fctmp, _ = f_reach(param,xtmp[tl],False)
cost = ftmp.flatten() @ Q @ ftmp.flatten() + fctmp.flatten() @ Qc @ fctmp.flatten() + np.linalg.norm(utmp) * param.r
if cost < cost0 or alpha < 1e-3:
@@ -192,10 +190,10 @@ plt.axis("off")
plt.gca().set_aspect('equal', adjustable='box')
# Get points of interest
-f = fkin(x)
-f00 = fkin0(x[0])
-f10 = fkin0(x[tl[0]])
-fT0 = fkin0(x[tl[1]])
+f = fkin(param,x)
+f00 = fkin0(param,x[0])
+f10 = fkin0(param,x[tl[0]])
+fT0 = fkin0(param,x[tl[1]])
plt.plot( f00[:,0] , f00[:,1],c='black',linewidth=5,alpha=.2)
plt.plot( f10[:,0] , f10[:,1],c='black',linewidth=5,alpha=.6)
diff --git a/python/iLQR_manipulator_obstacle.py b/python/iLQR_manipulator_obstacle.py
index 503f71d2fdcef5eff4ec0ef12435ec16a56d1db1..744586a10f9dd16b817c46612c18ec09e03e04ef 100644
--- a/python/iLQR_manipulator_obstacle.py
+++ b/python/iLQR_manipulator_obstacle.py
@@ -48,7 +48,7 @@ def fkin(x,ls):
return f.T
# Forward Kinematics for all joints
-def fkin0(x):
+def fkin0(x, param):
T = np.tril(np.ones([param.nbVarX,param.nbVarX]))
T2 = np.tril(np.matlib.repmat(param.linkLengths,len(x),1))
f = np.vstack((
@@ -62,8 +62,7 @@ def fkin0(x):
return f
# Forward Kinematics for all joints in batch
-def fkin0batch(x):
- global param
+def fkin0batch(x, param):
T = np.tril(np.ones([param.nbVarX,param.nbVarX]))
T2 = np.tril(np.matlib.repmat(param.linkLengths,x.shape[1],1))
f = np.vstack((
@@ -72,9 +71,8 @@ def fkin0batch(x):
)).T
return f
-# Cost and jacobians for avoiding obstacles
-def f_avoid(x):
- global param
+# Residual and Jacobian for an obstacle avoidance task
+def f_avoid(x, param):
f=[]
idx=[]
J=np.zeros((0,0))
@@ -104,13 +102,11 @@ def f_avoid(x):
idx = np.array(idx)
return f,J.T,idx
-# Cost and jacobians for avoiding obstacles, computed in batch
-def f_avoid2(x):
- global param
+def f_avoid2(x, param):
f, J, idx = [], [], []
for i in range(param.nbObstacles):
- f_x = fkin0batch(x)
+ f_x = fkin0batch(x, param)
e = (f_x- param.Obs[i][:2])@param.U_obs[i]
ftmp = 1 - np.linalg.norm(e, axis=1)**2
@@ -139,8 +135,8 @@ def f_avoid2(x):
J = J[:,idx]
return f, J,idx
+# Jacobian with analytical computation (for single time step)
def jkin(x,ls):
- global param
T = np.tril( np.ones((len(ls),len(ls))) )
J = np.vstack((
-np.sin(T@x).T @ np.diag(ls) @ T,
@@ -149,10 +145,8 @@ def jkin(x,ls):
))
return J
-# Cost and gradient
-def f_reach( x ):
- global param
-
+# Residual and Jacobian for a via-point reaching task
+def f_reach(x, param):
f = logmap(fkin(x,param.linkLengths),param.mu)
J = np.zeros(( len(x) * param.nbVarF , len(x) * param.nbVarX ))
@@ -177,7 +171,7 @@ param.sizeObstacle = [.5,.8] # Size of objects
param.r = 1e-3 # Control weight term
param.Q_track = 1e2
param.Q_avoid = 1e0
-param.useBatch = True #Batch computation for collision avoidance cost
+param.useBatch = False #Batch computation for collision avoidance cost
# Task parameters
# ===============================
@@ -239,11 +233,11 @@ for i in range( param.nbIter ):
x = Su0 @ u + Sx0 @ x0
x = x.reshape( ( param.nbData , param.nbVarX) )
- f, J = f_reach(x[tl]) # Tracking objective
+ f, J = f_reach(x[tl], param) # Tracking objective
if param.useBatch:
- f2, J2, id2 = f_avoid2(x)# Avoidance objective
+ f2, J2, id2 = f_avoid2(x, param)# Avoidance objective
else:
- f2, J2, id2 = f_avoid(x)# Avoidance objective
+ f2, J2, id2 = f_avoid(x, param)# Avoidance objective
if len(id2) > 0: # Numpy does not allow zero sized array as Indices
Su2 = Su0[id2.flatten()]
@@ -260,11 +254,11 @@ for i in range( param.nbIter ):
utmp = u + du * alpha
xtmp = Su0 @ utmp + Sx0 @ x0
xtmp = xtmp.reshape( ( param.nbData , param.nbVarX) )
- ftmp, _ = f_reach(xtmp[tl])
+ ftmp, _ = f_reach(xtmp[tl], param)
if param.useBatch:
- f2tmp,_,_ = f_avoid2(xtmp)
+ f2tmp,_,_ = f_avoid2(xtmp, param)
else:
- f2tmp,_,_ = f_avoid(xtmp)
+ f2tmp,_,_ = f_avoid(xtmp, param)
cost = np.linalg.norm(ftmp.flatten())**2 * param.Q_track + np.linalg.norm(f2tmp.flatten())**2 * param.Q_avoid + np.linalg.norm(utmp) * param.r
@@ -293,8 +287,8 @@ for i in range(param.nbVarX-1):
fi = fkin( x[:,:i+1] , param.linkLengths[:i+1] )
plt.plot(fi[:,0],fi[:,1],c="black",alpha=.5,linestyle='dashed')
-f00 = fkin0(x[0])
-fT0 = fkin0(x[-1])
+f00 = fkin0(x[0], param)
+fT0 = fkin0(x[-1],param)
plt.plot( f00[:,0] , f00[:,1],c='black',linewidth=5,alpha=.2)
plt.plot( fT0[:,0] , fT0[:,1],c='black',linewidth=5,alpha=.6)
diff --git a/python/iLQR_obstacle.py b/python/iLQR_obstacle.py
index e0e974326ef3b09193488c9bae43741dff177137..9396939758c7d545a31818f87b276f515b562abf 100644
--- a/python/iLQR_obstacle.py
+++ b/python/iLQR_obstacle.py
@@ -24,14 +24,14 @@ import numpy as np
import matplotlib.pyplot as plt
import matplotlib.patches as patches
-def f_reach(x):
- global param
+# Residual and Jacobian for a via point reaching task
+def f_reach(x,param):
f = x - param.Mu[:,:2]
J = np.identity(x.shape[1]*x.shape[0])
return f,J
-def f_avoid(x):
- global param
+# Residual and Jacobian for an obstacle avoidance task
+def f_avoid(x,param):
f=[]
idx=[]
idt=[]
@@ -71,8 +71,8 @@ param.nbVarX = 2 # State space dimension (x1,x2,x3)
param.nbVarU = 2 # Control space dimension (dx1,dx2,dx3)
param.sizeObstacle = [.4,.6] # Size of objects
param.r = 1e-3 # Control weight term
-param.Q_track = 1e2
-param.Q_avoid = 1e0
+param.Q_track = 1e2 # Reaching task weight term
+param.Q_avoid = 1e0 # Obstacle avoidance task weight term
# Task parameters
# ===============================
@@ -87,6 +87,7 @@ param.Obs = np.array([
[1,0.6,np.pi/4], # [x1,x2,o]
[2,2.5,-np.pi/6] # [x1,x2,o]
])
+
param.A_obs = np.zeros(( param.nbObstacles , 2 , 2 ))
param.S_obs = np.zeros(( param.nbObstacles , 2 , 2 ))
param.Q_obs = np.zeros(( param.nbObstacles , 2 , 2 ))
@@ -129,8 +130,8 @@ for i in range( param.nbIter ):
x = Su0 @ u + Sx0 @ x0
x = x.reshape( ( param.nbData , param.nbVarX) )
- f, J = f_reach(x[tl]) # Tracking objective
- f2, J2, id2 , _ = f_avoid(x)# Avoidance objective
+ f, J = f_reach(x[tl],param) # Tracking objective
+ f2, J2, id2 , _ = f_avoid(x,param)# Avoidance objective
if len(id2) > 0: # Numpy does not allow zero sized array as Indices
Su2 = Su0[id2.flatten()]
@@ -148,8 +149,8 @@ for i in range( param.nbIter ):
utmp = u + du * alpha
xtmp = Su0 @ utmp + Sx0 @ x0
xtmp = xtmp.reshape( ( param.nbData , param.nbVarX) )
- ftmp, _ = f_reach(xtmp[tl])
- f2tmp,_,_,_ = f_avoid(xtmp)
+ ftmp, _ = f_reach(xtmp[tl],param)
+ f2tmp,_,_,_ = f_avoid(xtmp,param)
cost = np.linalg.norm(ftmp.flatten())**2 * param.Q_track + np.linalg.norm(f2tmp.flatten())**2 * param.Q_avoid + np.linalg.norm(utmp) * param.r
if cost < cost0 or alpha < 1e-3: