void WorldSQP::compute_difference_block(DynamicSparseMatrix<double>& m) { m.resize(_size_each_state*(_num_worlds-1), _size_each_state*(_num_worlds-2)); for (int i = 0; i < _num_worlds-1; i++) { int num_rows_start = i*_size_each_state; for (int r = 0; r < _size_each_state; r++) { if (i != 0) m.coeffRef(num_rows_start+r,num_rows_start+r - _size_each_state) = 1; if (i != _num_worlds-2) m.coeffRef(num_rows_start+r,num_rows_start+r) = -1; } } }
void WorldSQP::compute_all_jacobians() { boost::thread_group group; for (int traj_ind = 0; traj_ind < _num_traj; traj_ind++) { cout << "Computing Jacobians on traj = " << traj_ind << endl; ; for (int i=0; i < current_states[traj_ind].size()-1; i++) { if (current_jacobians[traj_ind][i].rows() == 0) { group.create_thread( boost::bind(computeJacobian, current_states[traj_ind][i], ¤t_jacobians[traj_ind][i]) ); } } } group.join_all(); for (int traj_ind = 0; traj_ind < _num_traj; traj_ind++) { DynamicSparseMatrix<double> J; J.resize((_num_worlds-1)*_size_each_state, (_num_worlds-1)*_size_each_control); for (int i = 0; i < current_states[traj_ind].size()-1; i++) { int num_rows_start = i*_size_each_state; int num_cols_start = i*_size_each_control; for (int r=0; r < _size_each_state; r++) { for (int c=0; c < _size_each_control; c++) { J.coeffRef(num_rows_start+r, num_cols_start+c) = current_jacobians[traj_ind][i](r,c); } } //++progress; } block(_all_trans,traj_ind*_size_each_state*(_num_worlds-1), _cols_all_unknown_states, J); } }