Exemplo n.º 1
0
Eigen::ArrayXXd mpFlow::math::circularPoints(double const radius, double const distance,
    double const offset, bool const invertDirection, Eigen::Ref<Eigen::ArrayXd const> const midpoint) {
    Eigen::ArrayXd const phi = arange(offset / radius, offset / radius + 2.0 * M_PI, distance / radius);

    Eigen::ArrayXXd result = Eigen::ArrayXXd::Zero(phi.rows(), 2);
    result.col(0) = midpoint(0) + radius * phi.cos();
    result.col(1) = midpoint(1) + (invertDirection ? -1.0 : 1.0) * radius * phi.sin();

    return result;
}
Exemplo n.º 2
0
void RGRRTNode::FindReachableSet(){
    //Need to discretize control space, and fill in reachableStates and reachableControls arrays
    Eigen::ArrayXd contactPointLocations = Eigen::ArrayXd::LinSpaced(DISC_S,0,2*LO);
    Eigen::ArrayXd normalForceMagnitude = Eigen::ArrayXd::LinSpaced(DISC_FN,MIN_FN,MAX_FN);
    Eigen::ArrayXd tangentForceMagnitude = Eigen::ArrayXd::LinSpaced(DISC_FT,-1.0*MU,MU);
    Eigen::Matrix<double, CONTROL_SPACE_DIM,1> controlArray;
    int counter = 0;
    for(int i = 0; i < contactPointLocations.rows(); i++) {
	for(int j = 0; j < normalForceMagnitude.rows(); j++) {
	    for(int k = 0; k < tangentForceMagnitude.rows(); k++) {
		controlArray << contactPointLocations(i,0), \
		    normalForceMagnitude(j,0),		    \
		    tangentForceMagnitude(k,0)*normalForceMagnitude(j,0);
		reachableStates[counter] = spawn(nodeState,controlArray);
		reachableControls[counter] = controlArray;
		counter++;
	    }
	}
    }
}