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;
}
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++;
}
}
}
}
