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/matlab/iLQR_manipulator_obstacle.m b/matlab/iLQR_manipulator_obstacle.m
index cd7f986ea825a51531e0ffc5489326046f3273b7..803316391d56810959e69510bc9859c29b01b50f 100644
--- a/matlab/iLQR_manipulator_obstacle.m
+++ b/matlab/iLQR_manipulator_obstacle.m
@@ -2,7 +2,8 @@
%% (viapoints task with position+orientation including bounding boxes on f(x))
%%
%% Copyright (c) 2021 Idiap Research Institute, http://www.idiap.ch/
-%% Written by Sylvain Calinon <https://calinon.ch>
+%% Written by Teguh Lembono <teguh.lembono@idiap.ch>,
+%% Sylvain Calinon <https://calinon.ch>
%%
%% This file is part of RCFS.
%%
@@ -24,7 +25,7 @@ function iLQR_manipulator_obstacle
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
param.dt = 1E-2; %Time step size
param.nbData = 50; %Number of datapoints
-param.nbIter = 50; %Number of iterations for iLQR
+param.nbIter = 30; %Number of iterations for iLQR
param.nbPoints = 1; %Number of viapoints
param.nbObstacles = 2; %Number of obstacles
param.nbVarX = 3; %State space dimension (q1,q2,q3)
@@ -35,6 +36,7 @@ param.sz2 = [.5, .8]; %Size of obstacles
param.q = 1E2; %Tracking weight term
param.q2 = 1E0; %Obstacle avoidance weight term
param.r = 1E-3; %Control weight term
+param.useBatch = true; %use batch computation for collision cost
param.Mu = [[3; -1; 0]]; %Viapoints (x1,x2,o)
@@ -64,11 +66,16 @@ Su0 = [zeros(param.nbVarX, param.nbVarX*(param.nbData-1)); tril(kron(ones(param.
Sx0 = kron(ones(param.nbData,1), eye(param.nbVarX));
Su = Su0(idx,:);
+tic;
for n=1:param.nbIter
x = reshape(Su0 * u + Sx0 * x0, param.nbVarX, param.nbData); %System evolution
[f, J] = f_reach(x(:,tl), param); %Tracking objective
- [f2, J2, id2] = f_avoid(x, param); %Avoidance objective
+ if param.useBatch
+ [f2, J2, id2] = f_avoid2(x, param); %Avoidance objective
+ else
+ [f2, J2, id2] = f_avoid(x, param); %Avoidance objective
+ end
Su2 = Su0(id2,:);
du = (Su' * J' * J * Su * param.q + Su2' * J2' * J2 * Su2 * param.q2 + R) \ ...
@@ -81,7 +88,11 @@ for n=1:param.nbIter
utmp = u + du * alpha;
xtmp = reshape(Su0 * utmp + Sx0 * x0, param.nbVarX, param.nbData);
ftmp = f_reach(xtmp(:,tl), param); %Tracking objective
- ftmp2 = f_avoid(xtmp, param); %Avoidance objective
+ if param.useBatch
+ ftmp2 = f_avoid2(xtmp, param); %Avoidance objective
+ else
+ ftmp2 = f_avoid(xtmp, param); %Avoidance objective
+ end
cost = norm(ftmp(:))^2 * param.q + norm(ftmp2(:))^2 * param.q2 + norm(utmp)^2 * param.r;
if cost < cost0 || alpha < 1E-3
break;
@@ -89,13 +100,14 @@ for n=1:param.nbIter
alpha = alpha * 0.5;
end
u = u + du * alpha;
-
+ disp(cost)
if norm(du * alpha) < 1E-2
break; %Stop iLQR when solution is reached
end
end
disp(['iLQR converged in ' num2str(n) ' iterations.']);
-
+toc
+%disp(tEnd-tStart)
%Log data
r.x = x;
r.f = [];
@@ -192,6 +204,7 @@ function [f, J, id] = f_avoid(x, param)
f = [f; ftmp];
Jrob = [jkin(x(1:j,t), param.l(1:j)), zeros(param.nbVarF, param.nbVarU-j)];
Jtmp = -e' * param.U2(:,:,i)' * Jrob(1:2,:); %quadratic form
+
J = blkdiag(J, Jtmp);
id = [id, (t-1) * param.nbVarU + [1:param.nbVarU]'];
end
@@ -199,3 +212,46 @@ function [f, J, id] = f_avoid(x, param)
end
end
end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Forward kinematics for all robot articulations in batch
+function f = fkin0batch(x, L)
+ T = tril(ones(size(x,1)));
+ T2 = T * diag(L);
+ f = [(T2 * cos(T * x))(:), ...
+ (T2 * sin(T * x))(:)];
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Cost and gradient for an obstacle avoidance task with batch computation
+function [f, J, id] = f_avoid2(x, param)
+ f=[]; J=[]; id=[];
+ for i=1:param.nbObstacles
+ xee_batch = fkin0batch(x, param.l);
+ n= size(xee_batch,1);
+ e = (xee_batch - repmat(param.Mu2(1:2,i)', n , 1)) * param.U2(:,:,i);
+ ftmp = ones(n,1) - sum(e.^2, 2); %quadratic form
+
+ %get the indices where there is collision (ftmp > 0)
+ idx_active = linspace(1,n,n)(ftmp>0);
+
+ for idx=1:size(idx_active,2)
+ j = idx_active(idx);
+ t = floor((j-1)/param.nbVarX)+1;
+ k = mod(j-1,param.nbVarX)+1;
+
+ %compute Jacobian
+ Jrob = [jkin(x(1:k,t), param.l(1:k)), zeros(param.nbVarF, param.nbVarX-k)];
+ Jtmp = -e(j,:) * param.U2(:,:,i)' * Jrob(1:2,:); %quadratic form
+ Jj = zeros(1,param.nbVarX*param.nbData);
+ Jj(:,param.nbVarX*(t-1)+1:param.nbVarX*(t)) = Jtmp;
+ J = [J; Jj];
+ id = [id, (t-1) * param.nbVarX + [1:param.nbVarX]'];
+ end
+
+ f = [f; ftmp(idx_active(:))]; #keep only active value
+ end
+
+ id = unique(id); #remove duplicate
+ J = J(:,id); #keep only active value
+end
\ No newline at end of file
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_manipulator.py b/python/iLQR_manipulator.py
index e216452cc017ec53960efc4fbadfc71322528c9c..2aa963d47157c32745eb03efae5692997e361aa9 100644
--- a/python/iLQR_manipulator.py
+++ b/python/iLQR_manipulator.py
@@ -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
diff --git a/python/iLQR_manipulator_dynamics.py b/python/iLQR_manipulator_dynamics.py
index a0db108edde5fa223635a85301bb8d0afcc5f998..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,46 +35,43 @@ def logmap(f,f0):
return error
# Forward kinematics for E-E
-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),
- 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(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((
- T2 @ np.cos(T@x),
- T2 @ np.sin(T@x)
- )).T
- f = np.vstack((
- np.zeros(2),
- f
- ))
- return f
+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(param,x):
- T = np.tril( np.ones((len(x),len(x))) )
+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
# 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 ))
+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(x.shape[0]):
+ for t in range(param.nbPoints ):
f[t,:2] = param.A[t].T @ f[t,:2] # Object oriented fk
- Jtmp = jkin(param,x[t])
+ Jtmp = Jkin(x[t],param)
Jtmp[:2] = param.A[t].T @ Jtmp[:2] # Object centered jacobian
if param.useBoundingBox:
@@ -91,74 +87,69 @@ def f_reach(param,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
@@ -169,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(param,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
@@ -238,7 +217,7 @@ for i in range( param.nbIter ):
while True:
utmp = u + du * alpha
xtmp, _ = forward_dynamics(x, utmp, param)
- ftmp, _ = f_reach(param,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:
@@ -258,9 +237,9 @@ plt.axis("off")
plt.gca().set_aspect('equal', adjustable='box')
# Get points of interest
-f = fkin(param,x[:,:param.nbVarX])
-f00 = fkin0(param,x[0,:param.nbVarX])
-fT0 = fkin0(param,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)
@@ -273,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_obstacle.py b/python/iLQR_manipulator_obstacle.py
index 17eafe4518087ef48f9da4cb6c01af4d294bf347..ff77cbb4d52faede7812138a5f85c2a53f178288 100644
--- a/python/iLQR_manipulator_obstacle.py
+++ b/python/iLQR_manipulator_obstacle.py
@@ -3,6 +3,7 @@
Copyright (c) 2021 Idiap Research Institute, http://www.idiap.ch/
Written by Jeremy Maceiras <jeremy.maceiras@idiap.ch>,
+ Teguh Lembono <teguh.lembono@idiap.ch>,
Sylvain Calinon <https://calinon.ch>
This file is part of RCFS.
@@ -24,6 +25,8 @@ import numpy as np
import numpy.matlib
import matplotlib.pyplot as plt
import matplotlib.patches as patches
+import time
+
# Helper functions
# ===============================
@@ -37,15 +40,15 @@ def logmap(f,f0):
# Forward kinematics for E-E
def fkin(x,ls):
- x = x.T
- A = np.tril(np.ones([len(ls),len(ls)]))
- f = np.vstack((ls @ np.cos(A @ x),
- ls @ 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
+ x = x.T
+ A = np.tril(np.ones([len(ls),len(ls)]))
+ f = np.vstack((ls @ np.cos(A @ x),
+ ls @ 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
# Forward Kinematics for all joints
-def fkin0(param,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((
@@ -58,8 +61,18 @@ def fkin0(param,x):
))
return f
+# Forward Kinematics for all joints in batch
+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((
+ (T2 @ np.cos(T@x.T)).flatten(order='F'),
+ (T2 @ np.sin(T@x.T)).flatten(order='F')
+ )).T
+ return f
+
# Residual and Jacobian for an obstacle avoidance task
-def f_avoid(param,x):
+def f_avoid(x, param):
f=[]
idx=[]
J=np.zeros((0,0))
@@ -74,7 +87,7 @@ def f_avoid(param,x):
if ftmp > 0:
f += [ftmp]
- J_rbt = np.hstack(( jkin(x[t,:(j+1)] , param.linkLengths[:(j+1)]), np.zeros(( param.nbVarF , param.nbVarU-(j+1))) ))
+ J_rbt = np.hstack(( jkin(x[t,:(j+1)] , param.linkLengths[:(j+1)]), np.zeros(( param.nbVarF , param.nbVarX-(j+1))) ))
J_tmp = (-e.T @ param.U_obs[i].T @ J_rbt[:2]).reshape((-1,1))
#J_tmp = 1*(J_rbt[:2].T @ param.U_obs[i] @ e).reshape((-1,1))
@@ -83,12 +96,45 @@ def f_avoid(param,x):
J2[-J_tmp.shape[0]:,-J_tmp.shape[1]:] = J_tmp
J = J2 # Numpy does not provid a blockdiag function...
- idx.append( (t-1)*param.nbVarU + np.array(range(param.nbVarU)) )
+ idx.append( t*param.nbVarX + np.array(range(param.nbVarX)) )
f = np.array(f)
idx = np.array(idx)
return f,J.T,idx
+def f_avoid2(x, param):
+ f, J, idx = [], [], []
+
+ for i in range(param.nbObstacles):
+ 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
+
+ #check for value larger than zeros
+ idx_active = np.arange(len(ftmp))[ftmp>0]
+
+ #compute Jacobian for each idx_active
+ for j in idx_active:
+ t = j//param.nbVarX # which time step
+ k = j%param.nbVarX # which link
+
+ J_rbt = np.hstack((jkin(x[t,:(k+1)] , param.linkLengths[:(k+1)]), np.zeros(( param.nbVarF , param.nbVarX-(k+1))) ))
+ J_tmp = (-e[j].T @ param.U_obs[i].T @ J_rbt[:2])
+ J_j = np.zeros(param.nbVarX*param.nbData)
+ J_j[param.nbVarX*t: param.nbVarX*(t+1)] = J_tmp
+ J += [J_j]
+
+ #Get indices
+ idx += np.arange(param.nbVarX*t, param.nbVarX*(t+1)).tolist()
+ f += [ftmp[idx_active]]
+
+ idx = np.array(list(set(idx)))
+ f = np.concatenate(f)
+ if len(idx) > 0:
+ J = np.vstack(J)
+ J = J[:,idx]
+ return f, J,idx
+
# Jacobian with analytical computation (for single time step)
def jkin(x,ls):
T = np.tril( np.ones((len(ls),len(ls))) )
@@ -100,7 +146,7 @@ def jkin(x,ls):
return J
# Residual and Jacobian for a via-point reaching task
-def f_reach(param,x):
+def f_reach(x, param):
f = logmap(fkin(x,param.linkLengths),param.mu)
J = np.zeros(( len(x) * param.nbVarF , len(x) * param.nbVarX ))
@@ -125,6 +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 = False #Batch computation for collision avoidance cost
# Task parameters
# ===============================
@@ -181,13 +228,16 @@ Su = Su0[idx.flatten()] # We remove the lines that are out of interest
# Solving iLQR
# ===============================
-
+tic=time.time()
for i in range( param.nbIter ):
x = Su0 @ u + Sx0 @ x0
x = x.reshape( ( param.nbData , param.nbVarX) )
- f, J = f_reach(param,x[tl]) # Tracking objective
- f2, J2, id2 = f_avoid(param,x)# Avoidance objective
+ f, J = f_reach(x[tl], param) # Tracking objective
+ if param.useBatch:
+ f2, J2, id2 = f_avoid2(x, param)# Avoidance objective
+ else:
+ 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()]
@@ -204,8 +254,12 @@ 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(param,xtmp[tl])
- f2tmp,_,_ = f_avoid(param,xtmp)
+ ftmp, _ = f_reach(xtmp[tl], param)
+ if param.useBatch:
+ f2tmp,_,_ = f_avoid2(xtmp, param)
+ else:
+ 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:
@@ -216,8 +270,9 @@ for i in range( param.nbIter ):
alpha /=2
if np.linalg.norm(alpha * du) < 1e-2: # Early stop condition
- break
-
+ break
+toc=time.time()
+print('Solving in {} seconds'.format(toc-tic))
# Ploting
# ===============================
@@ -232,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(param,x[0])
-fT0 = fkin0(param,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)