bool Solver_LP_abstract::readLpFromFile(const std::string& filename,
VectorX &c, VectorX &lb, VectorX &ub,
MatrixXX &A, VectorX &Alb, VectorX &Aub)
{
std::ifstream in(filename.c_str(), std::ios::in | std::ios::binary);
typename MatrixXX::Index n=0, m=0;
in.read((char*) (&n),sizeof(typename MatrixXX::Index));
in.read((char*) (&m),sizeof(typename MatrixXX::Index));
c.resize(n);
lb.resize(n);
ub.resize(n);
A.resize(m,n);
Alb.resize(m);
Aub.resize(m);
in.read( (char *) c.data() , n*sizeof(typename MatrixXX::Scalar) );
in.read( (char *) lb.data() , n*sizeof(typename MatrixXX::Scalar) );
in.read( (char *) ub.data() , n*sizeof(typename MatrixXX::Scalar) );
in.read( (char *) A.data() , m*n*sizeof(typename MatrixXX::Scalar) );
in.read( (char *) Alb.data() , m*sizeof(typename MatrixXX::Scalar) );
in.read( (char *) Aub.data() , m*sizeof(typename MatrixXX::Scalar) );
in.close();
return true;
}
void cxy_icp_kinematic_chain<_Scalar>::getJacobian(MatrixXX& jacobian)
{
long rows = cxy_config::n_num_*getModelPointSize();
long cols = cxy_config::joint_DoFs;
if (rows != jacobian.rows()
|| cols != jacobian.cols())
{
jacobian.resize(rows, cols);
}
jacobian.setZero();
int row = 0;
for (int ii = 0; ii < points_.size(); ++ii)
{
row = ii*config_->n_num_;
//jacobian.row(1);
points_[ii]->computePointJacobian(jacobian.block(row, 0, cxy_config::n_num_, jacobian.cols()));
}
fout_res_<<"x = "<<std::endl;
fout_res_<<Rad2Deg(x_)<<std::endl;
fout_res_<<"jac = "<<std::endl;
fout_res_<<jacobian<<std::endl;
}