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/python/iLQR_bimanual_manipulability.py b/python/iLQR_bimanual_manipulability.py
index e446e3de085e6a382907cd22644e66eba8fb6d86..ec0c8bb8f8c3206d3bba5ca7b1fe4802d9c18dc3 100644
--- a/python/iLQR_bimanual_manipulability.py
+++ b/python/iLQR_bimanual_manipulability.py
@@ -1,5 +1,6 @@
'''
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
@@ -118,19 +119,19 @@ def f_manipulability(x, param):
return f, J
## Parameters
-class Param:
- def __init__(self):
- self.dt = 1e0 # Time step length
- self.nbIter = 100 # Maximum number of iterations for iLQR
- self.nbPoints = 1 # Number of viapoints
- self.nbData = 10 # Number of datapoints
- self.nbVarX = 5 # State space dimension ([q1,q2,q3] for left arm, [q1,q4,q5] for right arm)
- self.nbVarU = self.nbVarX # Control space dimension ([dq1, dq2, dq3, dq4, dq5])
- self.nbVarF = 4 # Objective function dimension ([x1,x2] for left arm and [x3,x4] for right arm)
- self.l = np.ones((self.nbVarX, 1)) * 2. # Robot links lengths
- self.r = 1e-6 # Control weight term
- self.MuS = np.asarray([[3, 2], [2, 4]])
-param = Param()
+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
@@ -145,7 +146,6 @@ 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