Skip to content

Commit

Permalink
update for the TRO submission
Browse files Browse the repository at this point in the history
  • Loading branch information
@quimortiz
quimortiz committed Nov 8, 2023
1 parent 780c90e commit 44cd812
Show file tree
Hide file tree
Showing 6 changed files with 95 additions and 53 deletions.
2 changes: 1 addition & 1 deletion dynobench
Submodule dynobench updated 9 files
+6 −0 include/dynobench/acrobot.hpp
+30 −0 include/dynobench/car.hpp
+1 −1 include/dynobench/math_utils.hpp
+5 −0 include/dynobench/planar_rotor.hpp
+7 −0 include/dynobench/planar_rotor_pole.hpp
+4 −0 include/dynobench/quadrotor.hpp
+21 −4 include/dynobench/robot_models_base.hpp
+4 −0 include/dynobench/unicycle1.hpp
+4 −0 include/dynobench/unicycle2.hpp
36 changes: 20 additions & 16 deletions include/dynoplan/dbastar/dbastar.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -170,31 +170,35 @@ struct LazyTraj {
assert(offset);
assert(robot);
assert(motion);

assert(offset->size());
if (forward) {
// robot->transform_primitive(*offset, motion->traj.states,
// motion->traj.actions, tmp, check_state,
// num_valid_states);
static_cast<dynobench::Model_quad3d *>(robot)->transform_primitiveDirect(
*offset, motion->traj.states, motion->traj.actions, tmp, check_state,
num_valid_states);
robot->transform_primitive(*offset, motion->traj.states,
motion->traj.actions, tmp, check_state,
num_valid_states);
// static_cast<dynobench::Model_quad3d
// *>(robot)->transform_primitiveDirect(
// *offset, motion->traj.states, motion->traj.actions, tmp,
// check_state, num_valid_states);

} else {
// TODO: change this outside translation invariance
throw std::runtime_error(
"bacward still needs a little bit testing -- don't use");

if (startsWith(robot->name, "quad2d")) {
// std::cout << "transforming primitive" << std::endl;
static_cast<dynobench::Model_quad2d *>(robot)
->transform_primitiveDirectReverse(*offset, motion->traj.states,
motion->traj.actions, tmp,
check_state, num_valid_states);

auto ptr = dynamic_cast<dynobench::Model_quad2d *>(robot);
assert(ptr);
ptr->transform_primitiveDirectReverse(*offset, motion->traj.states,
motion->traj.actions, tmp,
check_state, num_valid_states);

} else if (startsWith(robot->name, "quad3d")) {
static_cast<dynobench::Model_quad3d *>(robot)
->transform_primitiveDirectReverse(*offset, motion->traj.states,
motion->traj.actions, tmp,
check_state, num_valid_states);
auto ptr = dynamic_cast<dynobench::Model_quad3d *>(robot);
assert(ptr);
ptr->transform_primitiveDirectReverse(*offset, motion->traj.states,
motion->traj.actions, tmp,
check_state, num_valid_states);
}

else if (startsWith(robot->name, "unicycle")) {
Expand Down
12 changes: 9 additions & 3 deletions src/dbastar/dbastar.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -703,10 +703,8 @@ void dbastar(const dynobench::Problem &problem, Options_dbastar options_dbastar,
std::vector<AStarNode *> neighbors_n;
std::vector<Trajectory> expanded_trajs; // for debugging

const bool debug = false;
const bool debug = false; // Set to true to write save to disk a lot of stuff

// TODO: option to inflat the heuristic
//
const bool check_intermediate_goal = true;
const size_t num_check_goal =
4; // Eg, for n = 4 I check: 1/5 , 2/5 , 3/5 , 4/5
Expand Down Expand Up @@ -934,6 +932,14 @@ void dbastar(const dynobench::Problem &problem, Options_dbastar options_dbastar,
out2 << " - " << std::endl;
traj.to_yaml_format(out2, " ");
}

{
dynobench::Trajectories trajs;
trajs.data = expanded_trajs;
trajs.save_file_msgpack("/tmp/dynoplan/expanded_trajs.msgpack");
}

// write in msgpack format
}

std::cout << "Terminate status: " << static_cast<int>(status) << " "
Expand Down
38 changes: 38 additions & 0 deletions src/idbastar/idbastar.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,44 @@ void idbA(const dynobench::Problem &problem,
options_dbastar_local.motions_ptr = &motions;
std::cout << "Loading motion primitives -- DONE " << std::endl;

if (false) {
// TODO: QUIM think of good way to quickly check that primitives are
// OK -- should be robust against bounds on invariances
std::cout << "checking motion primitives" << std::endl;

auto good_motion = [&](auto &motion) {
for (size_t j = 0; j < motion.traj.states.size(); j++) {
if (!robot->check_state(motion.traj.states.at(j))) {
return false;
}
}
return true;
};

if (robot->name == "car_with_trailers") {
// TODO: double check that this works for all systems!
Eigen::VectorXd x_lb = robot->x_lb;
Eigen::VectorXd x_ub = robot->x_ub;

robot->x_lb.head(robot->get_translation_invariance()).array() =
-std::numeric_limits<double>::max();
robot->x_ub.head(robot->get_translation_invariance()).array() =
std::numeric_limits<double>::max();

std::cout << "motions before check" << motions.size() << std::endl;
motions.erase(std::remove_if(motions.begin(), motions.end(), good_motion),
motions.end());
std::cout << "motions after check" << motions.size() << std::endl;

for (size_t i = 0; i < motions.size(); i++) {
motions[i].idx = i;
}

robot->x_lb = x_lb;
robot->x_ub = x_ub;
}
}

std::vector<Heuristic_node> heu_map;
if (options_dbastar.heuristic == 1) {

Expand Down
1 change: 1 addition & 0 deletions src/ompl/rrt_to.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,6 +107,7 @@ void solve_ompl_geometric(const dynobench::Problem &problem,
Eigen::VectorXd x;
state_to_eigen(x, si, s);
robot->diff_model->set_0_velocity(x);
robot->diff_model->ensure(x);
traj_geo.states.push_back(x);
}
traj_geo.states.push_back(traj_geo.goal);
Expand Down
59 changes: 26 additions & 33 deletions src/optimization/ocp.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -205,18 +205,13 @@ void check_problem_with_finite_diff(

void add_noise(double noise_level, std::vector<Eigen::VectorXd> &xs,
std::vector<Eigen::VectorXd> &us,
const std::string &robot_type) {
std::shared_ptr<dynobench::Model_robot> model_robot) {
size_t nx = xs.at(0).size();
size_t nu = us.at(0).size();
for (size_t i = 0; i < xs.size(); i++) {
DYNO_CHECK_EQ(static_cast<size_t>(xs.at(i).size()), nx, AT);
xs.at(i) += noise_level * Vxd::Random(nx);

if (startsWith(robot_type, "quad3d")) {
for (auto &s : xs) {
s.segment<4>(3).normalize();
}
}
model_robot->ensure(xs.at(i));
}

for (size_t i = 0; i < us.size(); i++) {
Expand Down Expand Up @@ -481,7 +476,7 @@ void solve_for_fixed_penalty(

// add noise
if (options_trajopt_local.noise_level > 0.) {
add_noise(options_trajopt_local.noise_level, xs, us, problem.robotType);
add_noise(options_trajopt_local.noise_level, xs, us, model_robot);
}

// store init guess
Expand Down Expand Up @@ -524,18 +519,27 @@ void solve_for_fixed_penalty(

// report after
std::string filename = folder_tmptraj + "opt_" + random_id + ".yaml";
write_states_controls(ddp.get_xs(), ddp.get_us(), model_robot, problem,
filename.c_str());

for (auto &x : xs_out) {
model_robot->ensure(x);
}

write_states_controls(xs_out, us_out, model_robot, problem, filename.c_str());
report_problem(problem_croco, xs_out, us_out, "/tmp/dynoplan/report-1.yaml");
};

void __trajectory_optimization(
const dynobench::Problem &problem,
const dynobench::Problem &t_problem,
std::shared_ptr<dynobench::Model_robot> &model_robot,
const dynobench::Trajectory &init_guess,
const Options_trajopt &options_trajopt, dynobench::Trajectory &traj,
Result_opti &opti_out) {

dynobench::Problem problem = t_problem;

model_robot->ensure(problem.start); // precission issues with quaternions
model_robot->ensure(problem.goal);

Options_trajopt options_trajopt_local = options_trajopt;

std::vector<SOLVER> solvers{SOLVER::traj_opt,
Expand Down Expand Up @@ -612,7 +616,8 @@ void __trajectory_optimization(

std::vector<Eigen::VectorXd> xs_init__ = xs_init;

if (startsWith(problem.robotType, "quad3d")) {
if (startsWith(problem.robotType, "quad3d") ||
model_robot->name == "quad3d") {
fix_problem_quaternion(start, goal, xs_init, us_init);
}

Expand All @@ -626,10 +631,8 @@ void __trajectory_optimization(
}
}

if (startsWith(problem.robotType, "quad3d")) {
for (auto &s : xs_init) {
s.segment<4>(3).normalize();
}
for (auto &x : xs_init) {
model_robot->ensure(x);
}

write_states_controls(xs_init, us_init, model_robot, problem,
Expand Down Expand Up @@ -946,7 +949,7 @@ void __trajectory_optimization(
}

if (options_trajopt_local.noise_level > 1e-8) {
add_noise(options_trajopt_local.noise_level, xs, us, problem.robotType);
add_noise(options_trajopt_local.noise_level, xs, us, model_robot);
}

report_problem(problem_croco, xs, us, "/tmp/dynoplan/report-0.yaml");
Expand Down Expand Up @@ -1439,11 +1442,8 @@ void trajectory_optimization(const dynobench::Problem &problem,
tmp_init_guess = from_welf_to_quim(init_guess, robot_derived->u_nominal);
}

if (startsWith(problem.robotType, "quad3d")) {
for (auto &s : tmp_init_guess.states) {
s.segment<4>(3).normalize();
}
}
for (auto &s : tmp_init_guess.states)
model_robot->ensure(s);

CSTR_(model_robot->ref_dt);

Expand Down Expand Up @@ -1499,12 +1499,8 @@ void trajectory_optimization(const dynobench::Problem &problem,
init_guess.actions, init_guess.times,
model_robot->ref_dt, model_robot->state);

if (startsWith(problem.robotType, "quad3d")) {

for (auto &s : tmp_init_guess.states) {
s.segment<4>(3).normalize();
}
}
for (auto &s : tmp_init_guess.states)
model_robot->ensure(s);
}
CHECK(tmp_init_guess.actions.size(), AT);
CHECK(tmp_init_guess.states.size(), AT);
Expand Down Expand Up @@ -1733,11 +1729,8 @@ void trajectory_optimization(const dynobench::Problem &problem,

options_trajopt_local.solver_id = static_cast<int>(SOLVER::traj_opt);

if (startsWith(problem.robotType, "quad3d")) {
for (auto &s : traj_rate_i.states) {
s.segment<4>(3).normalize();
}
}
for (auto &s : traj_rate_i.states)
model_robot->ensure(s);

__trajectory_optimization(problem, model_robot, traj_rate_i,
options_trajopt_local, traj_out,
Expand Down

0 comments on commit 44cd812

Please sign in to comment.