//Start and stay at PowerOFF
void test_stateTransition0(void)
{
//All of these should pass
State = getInitialState();
State = Ubuntu_State[State]();
TEST_ASSERT_EQUAL(POWEROFF,State);
State = Ubuntu_State[State]();
TEST_ASSERT_EQUAL(POWEROFF,State);
}
TEST_P(ComponentIntegrationSchemeAndCountParamTest, TimeMatrixInitialization)
{
int numCubes;
SurgSim::Math::IntegrationScheme integrationScheme;
SurgSim::Math::LinearSolver linearSolver;
std::tie(integrationScheme, linearSolver, numCubes) = GetParam();
RecordProperty("IntegrationScheme", IntegrationSchemeNames[integrationScheme]);
RecordProperty("LinearSolver", LinearSolverNames[linearSolver]);
RecordProperty("CubeDivisions", boost::to_string(numCubes));
auto fem = std::make_shared<DivisibleCubeRepresentation>("cube", numCubes);
// We need to add some boundary conditions for the static solver to not run into a singular matrix
std::const_pointer_cast<SurgSim::Math::OdeState>(fem->getInitialState())->addBoundaryCondition(0);
std::const_pointer_cast<SurgSim::Math::OdeState>(fem->getInitialState())->addBoundaryCondition(1);
std::const_pointer_cast<SurgSim::Math::OdeState>(fem->getInitialState())->addBoundaryCondition(2);
fem->setIntegrationScheme(integrationScheme);
fem->setLinearSolver(linearSolver);
timeInitializeRepresentation(fem);
}
std::map<Key, sem_elem_t>
WFA::readOutCombineOverAllPathsValues(std::set<Key> const & alpha)
{
Key const init = getInitialState();
sem_elem_t weight = getSomeWeight()->zero();
std::map<Key, sem_elem_t> values_map;
for (std::set<Key>::const_iterator iter = alpha.begin();
iter != alpha.end(); iter++)
{
// Compute Combine_{t = (init,*iter,to)} (weight(t) extend weight(to))
// or Combine_{t = (init,*iter,to)} (weight(to) extend weight(t)),
// depending on whether the WFA holds a prestar or poststar
// answer.
//
// Get the set of transitions in the WFA that are of the
// form (init,*iter,?)
TransSet transitionSet = match(init, *iter);
for (TransSet::iterator tsiter = transitionSet.begin();
tsiter != transitionSet.end() ; tsiter++)
{
ITrans * t = *tsiter;
// Set tmp to (weight(to) extend weight(t)) or
// (weight(t) extend weight(to)),
// depending on whether the WFA holds a prestar or
// poststar answer.
sem_elem_t tmp;
sem_elem_t target_weight = getState(t->to())->weight();
sem_elem_t trans_weight = t->weight();
if (getQuery() == WFA::INORDER) {
tmp = trans_weight->extend(target_weight);
}
else {
tmp = target_weight->extend(trans_weight);
}
// Combine the value to the accumulated weight of all
// "to" states
weight = weight->combine(tmp);
}
values_map[*iter] = weight;
}
return values_map;
}
vector<pair<double, double> > System::simulate(){
vector<pair<double, double> > trace ;
gsl_odeiv2_system sys = {func, jac, 2, &mu};
gsl_odeiv2_driver * d = gsl_odeiv2_driver_alloc_y_new (&sys, gsl_odeiv2_step_rk8pd, 1e-6, 1e-6, 0.0);
double t = 0.0, t1 = 20.0;
double* state = getInitialState();
double y[2] = { state[0], state[1] };
for (int i = 1; i <= 1000; i++){
double ti = i * t1 / 1000.0;
int status = gsl_odeiv2_driver_apply (d, &t, ti, y);
if (status != GSL_SUCCESS){
printf ("error, return value=%d\n", status);
break;
}
trace.push_back(make_pair(y[0], y[1]));
//printf ("%.5e %.5e %.5e\n", t, y[0], y[1]);
}
delete state;
gsl_odeiv2_driver_free (d);
return trace;
}
void DescentOptimizer::initialize() {
cout << "Duplex initialization started. It make take a while to analyze the "
"root."
<< endl;
double *init = getInitialState();
max = new double[objectiveDimension];
min = new double[objectiveDimension];
vector<string> objectiveMinStringVector =
settings->listValues("objective", "uid-objective.min");
vector<string> objectiveMaxStringVector =
settings->listValues("objective", "uid-objective.max");
for (int i = 0; i < objectiveDimension; i++) {
min[i] = stod(objectiveMinStringVector[i]);
max[i] = stod(objectiveMaxStringVector[i]);
}
stat = new Stat(settings, max, min, opt);
// Setting up the root node
State *root = new State(parameterDimension, objectiveDimension);
root->setParameter(init);
root->setID(0);
root->setParentID(-1);
system->eval(root);
insert(0, root, root);
}
int main(int argc, char* argv[]) {
gflags::SetUsageMessage("[options] full_path_to_urdf_or_sdf_file");
gflags::ParseCommandLineFlags(&argc, &argv, true);
if (argc < 2) {
gflags::ShowUsageWithFlags(argv[0]);
return 1;
}
logging::HandleSpdlogGflags();
// todo: consider moving this logic into the RigidBodySystem class so it can
// be reused
FloatingBaseType floating_base_type = kQuaternion;
if (FLAGS_base == "kFixed") {
floating_base_type = kFixed;
} else if (FLAGS_base == "RPY") {
floating_base_type = kRollPitchYaw;
} else if (FLAGS_base == "QUAT") {
floating_base_type = kQuaternion;
} else {
throw std::runtime_error(string("Unknown base type") + FLAGS_base +
"; must be kFixed, RPY, or QUAT");
}
auto rigid_body_sys = make_shared<RigidBodySystem>();
rigid_body_sys->AddModelInstanceFromFile(argv[argc - 1], floating_base_type);
auto const& tree = rigid_body_sys->getRigidBodyTree();
if (FLAGS_add_flat_terrain) {
SPDLOG_TRACE(drake::log(), "adding flat terrain");
double box_width = 1000;
double box_depth = 10;
DrakeShapes::Box geom(Vector3d(box_width, box_width, box_depth));
Isometry3d T_element_to_link = Isometry3d::Identity();
T_element_to_link.translation() << 0, 0,
-box_depth / 2; // top of the box is at z=0
RigidBody<double>& world = tree->world();
Vector4d color;
color << 0.9297, 0.7930, 0.6758,
1; // was hex2dec({'ee','cb','ad'})'/256 in matlab
world.AddVisualElement(
DrakeShapes::VisualElement(geom, T_element_to_link, color));
tree->addCollisionElement(
DrakeCollision::Element(geom, T_element_to_link, &world), world,
"terrain");
tree->compile();
}
shared_ptr<lcm::LCM> lcm = make_shared<lcm::LCM>();
auto visualizer =
make_shared<BotVisualizer<RigidBodySystem::StateVector>>(lcm, tree);
auto sys = cascade(rigid_body_sys, visualizer);
SimulationOptions options;
options.realtime_factor = 1.0;
options.timeout_seconds = std::numeric_limits<double>::infinity();
options.initial_step_size = 5e-3;
runLCM(sys, lcm, 0, std::numeric_limits<double>::infinity(),
getInitialState(*sys), options);
// simulate(*sys, 0, std::numeric_limits<double>::max(),
// getInitialState(*sys), options);
return 0;
}
