Skip to content

Commit

Permalink
Updated to use templated DSL interface
Browse files Browse the repository at this point in the history
  • Loading branch information
@msheckells
msheckells committed Dec 30, 2015
1 parent 32ec6d7 commit 0029112
Show file tree
Hide file tree
Showing 3 changed files with 35 additions and 21 deletions.
2 changes: 1 addition & 1 deletion bin/body3ddem.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,7 +90,7 @@ void solver_process(Viewer* viewer)

double temp[50000];

Body3dPqpDem pqp(dem, .1);
Body3dPqpDem pqp(dem, 1);

double temp2[50000];

Expand Down
23 changes: 14 additions & 9 deletions bin/body3ddemstab.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,8 @@
#include "params.h"
#include "dsl/gridsearch.h"
#include "dsl/gridcost.h"
#include "dsl/grid2d.h"
#include "dsl/grid2dconnectivity.h"

/*
Tests Gyroscopic obstacle avoidance for a rigid body in a geometric terrain
Expand All @@ -28,28 +30,29 @@ Params params;


// produce a geometric trajectory b/n x0 and xf on a dem using grid D*-Lite
void GetTraj(vector<Body3dState> &gxs, Dem &dem, GridSearch &gdsl, const Body3dState &x0, const Body3dState &xf) {
void GetTraj(vector<Body3dState> &gxs, Dem &dem, GridSearch<2> &gdsl,
const Body3dState &x0, const Body3dState &xf) {

int i0,j0,ig,jg;
dem.Point2Index(i0, j0, x0.p[0], x0.p[1]);
dem.Point2Index(ig, jg, xf.p[0], xf.p[1]);
gdsl.SetStart(j0, i0);
gdsl.SetGoal(jg, ig);
GridPath path, optPath;
gdsl.SetStart(Vector2d(j0, i0));
gdsl.SetGoal(Vector2d(jg, ig));
GridPath<2> path, optPath;
gdsl.Plan(path);
gdsl.OptPath(path, optPath, 2);
for (int i = 0; i < optPath.cells.size(); ++i) {
Body3dState x = xf;
dem.Index2Point(x.p[0], x.p[1], optPath.cells[i][1], optPath.cells[i][0]);
dem.Index2Point(x.p[0], x.p[1], optPath.cells[i].c[1], optPath.cells[i].c[0]);
x.p[2] = x0.p[2];

// if not last point
Vector3d v(0,0,0);
if (i < optPath.cells.size() - 1) {
Vector3d pa;
Vector3d pb;
dem.Index2Point(pa[0], pa[1], optPath.cells[i][1], optPath.cells[i][0]);
dem.Index2Point(pb[0], pb[1], optPath.cells[i+1][1], optPath.cells[i+1][0]);
dem.Index2Point(pa[0], pa[1], optPath.cells[i].c[1], optPath.cells[i].c[0]);
dem.Index2Point(pb[0], pb[1], optPath.cells[i+1].c[1], optPath.cells[i+1].c[0]);
pa[2] = x0.p[2];
pb[2] = x0.p[2];
Vector3d v = pb - pa;
Expand Down Expand Up @@ -174,8 +177,10 @@ void solver_process(Viewer* viewer)
double temp2[50000];

vector<Body3dState> gxs;
GridCost gridcost;
GridSearch gdsl(dem.nj, dem.ni, gridcost, dem.data);
Grid2d grid(dem.nj, dem.ni, dem.data, 1, 1, 1, 1);
Grid2dConnectivity grid_connectivity(grid);
GridCost<2> gridcost;
GridSearch<2> gdsl(grid, grid_connectivity, gridcost);
GetTraj(gxs, dem, gdsl, x0, xf);

Body3dView<6> dslView(sys, &gxs);
Expand Down
31 changes: 20 additions & 11 deletions lib/algos/body3ddemcontroller.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,8 @@

#include "dsl/gridsearch.h"
#include "dsl/gridcost.h"
#include "dsl/grid2d.h"
#include "dsl/grid2dconnectivity.h"
#include "body3davoidcontroller.h"
#include "pqpdem.h"
#include "controller.h"
Expand Down Expand Up @@ -52,13 +54,16 @@ namespace gcop {

// virtual bool SetContext(const Body3dPqpDem &c);

void GetTraj(vector<Body3dState> &xds, const Dem &dem, GridSearch &gdsl, const Body3dState &x0, const Body3dState &xf, double vd);
void GetTraj(vector<Body3dState> &xds, const Dem &dem,
GridSearch<2> &gdsl, const Body3dState &x0, const Body3dState &xf, double vd);

const Body3dState &x0; ///< start state
Body3dPqpDem &dem; ///< gidital elevation map
Body3dAvoidController<nu> localCtrl; ///< local avoidance controller
GridSearch dsl; ///< global grid search
GridCost gridcost; ///< grid cost
GridSearch<2> dsl; ///< global grid search
GridCost<2> gridcost; ///< grid cost
Grid2dConnectivity grid_connectivity;
Grid2d grid;
vector<Body3dState> xds; ///< computed sequence of waypoints
double vd; ///< desired forward velocity
int j; ///< waypoint index
Expand All @@ -67,28 +72,29 @@ namespace gcop {


template <int nu>
void Body3dDemController<nu>::GetTraj(vector<Body3dState> &xds, const Dem &dem, GridSearch &gdsl, const Body3dState &x0, const Body3dState &xf, double vd) {
void Body3dDemController<nu>::GetTraj(vector<Body3dState> &xds, const Dem &dem, GridSearch<2> &gdsl,
const Body3dState &x0, const Body3dState &xf, double vd) {

int i0,j0,ig,jg;
dem.Point2Index(i0, j0, x0.p[0], x0.p[1]);
dem.Point2Index(ig, jg, xf.p[0], xf.p[1]);
gdsl.SetStart(j0, i0);
gdsl.SetGoal(jg, ig);
GridPath path, optPath;
gdsl.SetStart(Vector2d(j0, i0));
gdsl.SetGoal(Vector2d(jg, ig));
GridPath<2> path, optPath;
gdsl.Plan(path);
gdsl.OptPath(path, optPath, 2);
for (int i = 0; i < optPath.cells.size(); ++i) {
Body3dState x = xf;
dem.Index2Point(x.p[0], x.p[1], optPath.cells[i][1], optPath.cells[i][0]);
dem.Index2Point(x.p[0], x.p[1], optPath.cells[i].c[1], optPath.cells[i].c[0]);
x.p[2] = x0.p[2];

// if not last point
Vector3d v(0,0,0);
if (i > 0) {
Vector3d pa;
Vector3d pb;
dem.Index2Point(pa[0], pa[1], optPath.cells[i-1][1], optPath.cells[i-1][0]);
dem.Index2Point(pb[0], pb[1], optPath.cells[i][1], optPath.cells[i][0]);
dem.Index2Point(pa[0], pa[1], optPath.cells[i-1].c[1], optPath.cells[i-1].c[0]);
dem.Index2Point(pb[0], pb[1], optPath.cells[i].c[1], optPath.cells[i].c[0]);
pa[2] = x0.p[2];
pb[2] = x0.p[2];
v = pb - pa;
Expand All @@ -107,7 +113,10 @@ namespace gcop {
Body3dState *xf,
Body3dPqpDem &dem,
double vd) :
x0(x0), dem(dem), localCtrl(sys, xf, 0, &dem), dsl(dem.dem.nj, dem.dem.ni, gridcost, dem.dem.data), vd(vd), j(0), wpRadius(10)
x0(x0), dem(dem), localCtrl(sys, xf, 0, &dem),
grid(dem.dem.nj, dem.dem.ni, dem.dem.data, 1, 1, 1, 1),
grid_connectivity(grid),
dsl(grid, grid_connectivity, gridcost), vd(vd), j(0), wpRadius(10)
{
}

Expand Down

0 comments on commit 0029112

Please sign in to comment.