int mat2csv(std::string file_path, MatX data) { std::ofstream outfile(file_path); // open file if (outfile.good() != true) { printf(E_CSV_DATA_OPEN, file_path.c_str()); return -1; } // save matrix for (int i = 0; i < data.rows(); i++) { for (int j = 0; j < data.cols(); j++) { outfile << data(i, j); if ((j + 1) != data.cols()) { outfile << ","; } } outfile << "\n"; } // close file outfile.close(); return 0; }
IGL_INLINE void igl::slice( const MatX& X, const Eigen::PlainObjectBase<DerivedR> & R, const int dim, MatY& Y) { Eigen::Matrix<typename DerivedR::Scalar,Eigen::Dynamic,1> C; switch(dim) { case 1: // boring base case if(X.cols() == 0) { Y.resize(R.size(),0); return; } igl::colon(0,X.cols()-1,C); return slice(X,R,C,Y); case 2: // boring base case if(X.rows() == 0) { Y.resize(0,R.size()); return; } igl::colon(0,X.rows()-1,C); return slice(X,C,R,Y); default: assert(false && "Unsupported dimension"); return; } }
void ConstantConstraint::evaluateConstraints(const MatX& qt, MatX& h, MatX& H) { assert(qt.rows() == 1 || qt.cols() == 1); size_t DoF = std::max(qt.rows(), qt.cols()); if (size_t(h.rows()) != numCons || h.cols() != 1) { h.resize(numCons, 1); } if (size_t(H.rows()) != numCons || size_t(H.cols()) != DoF) { H.resize(numCons, DoF); } H.setZero(); for(size_t i=0; i<numCons; i++){ assert(index[i] < DoF); h(i) = qt(index[i])-value[i]; H(i,index[i]) = 1; } }
//Evaluate the constraints for Chompification void TSRConstraint::evaluateConstraints(const MatX& qt, MatX& h, MatX& H) { //the pos is equivalent to the position of the // end-effector in the robot frame. double xyzrpy[6]; //the position of the ee. Transform pos; //get the position of the ee forwardKinematics( qt, pos ); //get the position of the ee in the TSR frame endeffectorToTSRFrame( pos, xyzrpy ); //the dimensionality of the configuration space size_t DoF = qt.size() ; //format h (constraint value vector). if (size_t(h.rows()) != _dim_constraint || h.cols() != 1) { h.resize(_dim_constraint, 1); } int current_dim = 0; std::vector< int > active_dims; for ( int i = 0; i < _dim_constraint; i ++ ) { const int dim = _dimension_id[i]; //if the robot's position goes over the TSR's upper bound: if ( xyzrpy[dim] > _Bw(dim, 1) ){ h(current_dim) = xyzrpy[ dim ] - _Bw(dim, 1); active_dims.push_back( dim ); current_dim ++; } //if the robot's position goes below the TSR's lower bound: else if ( xyzrpy[dim] < _Bw( dim, 0 ) ){ h(current_dim) = xyzrpy[ dim ] - _Bw(dim, 0); active_dims.push_back( dim ); current_dim ++; } } if ( current_dim != _dim_constraint ){ h.conservativeResize( current_dim, 1 ); } if ( H.rows() != current_dim || size_t( H.cols() ) != DoF ){ H.resize( current_dim, DoF ); } computeJacobian( qt, pos, H, active_dims ); }
void mat2cvmat(MatX A, cv::Mat &B) { B = cv::Mat(A.rows(), A.cols(), CV_8UC1); for (int i = 0; i < A.rows(); i++) { for (int j = 0; j < A.cols(); j++) { B.at<double>(i, j) = A(i, j); } } }
virtual void evaluateConstraints(const MatX& qt, MatX& h, MatX& H){ assert((qt.cols()==1 && qt.rows()==2) || (qt.cols() == 2 && qt.rows() == 1)); h.conservativeResize(1,1); H.conservativeResize(1,2); h(0) = mydot(qt,qt) - 4; for(int i=0; i<2; i++){ H(i) = 2*qt(i); } }
virtual double getCost(const MatX& q, size_t body_index, MatX& dx_dq, MatX& cgrad ) { assert( (q.rows() == 2 && q.cols() == 1) || (q.rows() == 1 && q.cols() == 2) ); dx_dq.resize(3, 2); dx_dq.setZero(); dx_dq << 1, 0, 0, 1, 0, 0; cgrad.resize(3, 1); vec3f g; float c = map->sampleCost(vec3f(q(0), q(1), 0.0), g); cgrad << g[0], g[1], 0.0; return c; }