Skip to content

Commit

Permalink
Merge branch 'master' of git.lcsr.jhu.edu:mkobila1/gcop
Browse files Browse the repository at this point in the history
  • Loading branch information
@marinkobi
marinkobi committed Sep 25, 2015
2 parents 300e886 + b103c13 commit 8d35b9a
Show file tree
Hide file tree
Showing 10 changed files with 310 additions and 38 deletions.
16 changes: 10 additions & 6 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,10 +64,11 @@ endif(NOT (CMAKE_SIZEOF_VOID_P EQUAL 8) )
#find_package(Eigen REQUIRED)
#include_directories(${Eigen_INCLUDE_DIRS})

#find_package( OpenCV 2.4 COMPONENTS core highgui imgproc calib3d)
#include_directories( ${OpenCV_INCLUDE_DIRS} )
#set(EST_LIBS ${EST_LIBS} ${OpenCV_LIBS})
#message(STATUS "Hello ${OpenCV_LIBS}")
find_package( OpenCV 2.4 COMPONENTS core highgui imgproc calib3d)
IF (OpenCV_FOUND)
include_directories( ${OpenCV_INCLUDE_DIRS} )
set(EST_LIBS ${EST_LIBS} ${OpenCV_LIBS})
ENDIF (OpenCV_FOUND)

find_package(DC1394)
include_directories(${DC1394_INCLUDE_DIR})
Expand All @@ -83,8 +84,11 @@ include_directories(${GLU_INCLUDE_DIRS})
set(VIEWER_LIBS ${VIEWER_LIBS} ${GLU_LIBRARIES} ${GLU_LIBRARY})

find_package(DSL)
include_directories(${DSL_INCLUDE_DIRS})
set(VIEWER_LIBS ${VIEWER_LIBS} ${DSL_LIBRARIES} ${DSL_LIBRARY})

IF (DSL_FOUND)
include_directories(${DSL_INCLUDE_DIRS})
set(VIEWER_LIBS ${VIEWER_LIBS} ${DSL_LIBRARIES} ${DSL_LIBRARY})
ENDIF (DSL_FOUND)

find_package(GLUT REQUIRED)
include_directories(${GLUT_INCLUDE_DIRS})
Expand Down
4 changes: 2 additions & 2 deletions README
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,6 @@

4.) Use install.bash for compiling the package

5.) Required Dependencies: DSL (https://git.lcsr.jhu.edu/ggarime1/dsl) GSL (libgsl0-dev) Eigen3 (libeigen3-dev)
5.) Required Dependencies: GSL (libgsl0-dev) Eigen3 (libeigen3-dev)

6.) Optional Dependencies: OpenCV, OpenGL (For bins)
6.) Optional Dependencies: OpenCV, OpenGL (For bins) DSL (https://github.com/jhu-asco/dsl_gridsearch)
6 changes: 3 additions & 3 deletions bin/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,9 +67,6 @@ IF (PQP_FOUND)
add_executable(body3drhc body3drhc.cc)
target_link_libraries(body3drhc gcop_algos gcop_views gcop_systems ${ALL_LIBS})

add_executable(body3ddemstab body3ddemstab.cc)
target_link_libraries(body3ddemstab gcop_algos gcop_views gcop_systems ${ALL_LIBS})

add_executable(body3dcestab body3dcestab.cc)
target_link_libraries(body3dcestab gcop_algos gcop_views gcop_systems ${ALL_LIBS})

Expand All @@ -78,6 +75,9 @@ IF (DSL_FOUND)
add_executable(body3dcedemstab body3dcedemstab.cc)
target_link_libraries(body3dcedemstab gcop_algos gcop_views gcop_systems ${ALL_LIBS})

add_executable(body3ddemstab body3ddemstab.cc)
target_link_libraries(body3ddemstab gcop_algos gcop_views gcop_systems ${ALL_LIBS})

ENDIF (DSL_FOUND)


Expand Down
16 changes: 8 additions & 8 deletions lib/algos/gndocp.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,7 @@ using ceres::Solve;
* This is necessary only if xs was not already generated from us.
*/
GnDocp(System<T, _nx, _nu, _np> &sys,
LqCost<T, _nx, _nu, _np, _ng> &cost, Tparam<T, _nx, _nu, _np, _ntp> &tparam,
LsCost<T, _nx, _nu, _np, _ng> &cost, Tparam<T, _nx, _nu, _np, _ntp> &tparam,
vector<double> &ts, vector<T> &xs, vector<Vectorcd> &us,
Vectormd *p = 0, bool update = true);

Expand Down Expand Up @@ -200,7 +200,7 @@ struct Functor
const T& x = docp->xs[k];
const Vectorcd& u = docp->us[k];

((LqCost<T, _nx, _nu, _np, _ng>&)docp->cost).Res(g, t, x, u, h, docp->p);
((LsCost<T, _nx, _nu, _np, _ng>&)docp->cost).Res(g, t, x, u, h, docp->p);
//double cost_debug = ((LqCost<T, _nx, _nu, _np, _ng>&)docp->cost).L(t, x, u, h, docp->p);
memcpy(fvec.data() + i, g.data(), g.size()*sizeof(double));
i += g.size();
Expand All @@ -219,7 +219,7 @@ struct Functor
*/
}

((LqCost<T, _nx, _nu, _np, _ng>&)docp->cost).Res(g,
((LsCost<T, _nx, _nu, _np, _ng>&)docp->cost).Res(g,
docp->ts.back(),
docp->xs.back(),
docp->us.back(), 0);
Expand Down Expand Up @@ -268,17 +268,17 @@ struct Functor
const T& x = docp.xs[k];
const Vectorcd& u = docp.us[k];

((LqCost<T, _nx, _nu>&)docp.cost).ResX(rx, t, x, h);
((LsCost<T, _nx, _nu>&)docp.cost).ResX(rx, t, x, h);
memcpy(residuals + i, rx.data(), rx.size()*sizeof(double));
i += rx.size();

((LqCost<T, _nx, _nu>&)docp.cost).ResU(ru, t, u, h);
((LsCost<T, _nx, _nu>&)docp.cost).ResU(ru, t, u, h);
memcpy(residuals + i, ru.data(), ru.size()*sizeof(double));
i += ru.size();
}
if (docp.ts.size() > 1) {
double h = docp.ts.back() - docp.ts[docp.ts.size() - 2];
((LqCost<T, _nx, _nu>&)docp.cost).ResX(rx, docp.ts.back(), docp.xs.back(), h);
((LsCost<T, _nx, _nu>&)docp.cost).ResX(rx, docp.ts.back(), docp.xs.back(), h);
memcpy(residuals + i, rx.data(), rx.size()*sizeof(double));
}
return true;
Expand All @@ -291,7 +291,7 @@ struct Functor

template <typename T, int _nx, int _nu, int _np, int _ng, int _ntp>
GnDocp<T, _nx, _nu, _np, _ng, _ntp>::GnDocp(System<T, _nx, _nu, _np> &sys,
LqCost<T, _nx, _nu, _np, _ng> &cost,
LsCost<T, _nx, _nu, _np, _ng> &cost,
Tparam<T, _nx, _nu, _np, _ntp> &tparam,
vector<double> &ts,
vector<T> &xs,
Expand Down Expand Up @@ -373,7 +373,7 @@ struct Functor
// google::InitGoogleLogging(argv[0]);

int npar = this->sys.U.n*this->us.size();
int nres = ((LqCost<T, _nx, _nu, _np, _ng>&)cost).gn*this->xs.size();
int nres = ((LsCost<T, _nx, _nu, _np, _ng>&)cost).gn*this->xs.size();

// The variable to solve for with its initial value. It will be
// mutated in place by the solver.
Expand Down
3 changes: 2 additions & 1 deletion lib/systems/constraints/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,11 +4,12 @@ set(sources
set(headers
shell.h
cylinder.h
constraint.h
)

IF (PQP_FOUND)
set(headers
${headers}
pqpdem.h
)
ENDIF (PQP_FOUND)
ENDIF (PQP_FOUND)
0 lib/systems/constraint.h → lib/systems/constraints/constraint.h
View file
File renamed without changes.
33 changes: 19 additions & 14 deletions lib/systems/constraints/cylinder.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,13 +9,13 @@ namespace gcop {
int _nx = Dynamic,
int _nu = Dynamic,
int _np = Dynamic>
class Cylinder : public Constraint<T, _nx, _nu, _np, 1> {
class Cylinder : public Constraint<T, _nx, _nu, _np> {
public:

typedef Matrix<double, 1, 1> Vectorgd;
typedef Matrix<double, 1, _nx> Matrixgxd;
typedef Matrix<double, 1, _nu> Matrixgud;
typedef Matrix<double, 1, _np> Matrixgpd;
typedef Matrix<double, Dynamic, 1> Vectorgd;
typedef Matrix<double, Dynamic, _nx> Matrixgxd;
typedef Matrix<double, Dynamic, _nu> Matrixgud;
typedef Matrix<double, Dynamic, _np> Matrixgpd;

typedef Matrix<double, _nx, 1> Vectornd;
typedef Matrix<double, _nu, 1> Vectorcd;
Expand All @@ -40,8 +40,9 @@ namespace gcop {

template <typename T, int _nx, int _nu, int _np>
Cylinder<T, _nx, _nu, _np>::Cylinder(const Vector3d &o, double r, double h, double cr) :
Constraint<T, _nx, _nu, _np, 1>(), o(o), r(r), h(h), cr(cr)
Constraint<T, _nx, _nu, _np>(1), o(o), r(r), h(h), cr(cr)
{
this->ng = 1;
}

template <typename T, int _nx, int _nu, int _np>
Expand All @@ -51,24 +52,28 @@ namespace gcop {
Matrixgxd *dgdx, Matrixgud *dgdu,
Matrixgpd *dgdp)
{
const Vector3d &p = x.second.head(3); // position
g.resize(1);
const Vector3d &p = x.p; // position

Vector2d v = p.head<2>() - o.head<2>();
Vector3d v = p - o;
if(p(2) < o(2) + h && p(2) > o(2))
v(2) = 0;

double d = v.norm() - cr; // distance from center of cylinder to system boundary

if (d < 0) {
cout << "ERR: already colliding" << endl;
}
//if (d < 0) {
// cout << "ERR: already colliding" << endl;
//}

g[0] = r - d; // must be negative for non-collision

if (dgdx) {
dgdx->Zero();
dgdx->resize(1, _nx);
dgdx->setZero();
if (g[0] > 0)
dgdx->head(2) = -v/v.norm();
dgdx->row(0).head(3) = -v/v.norm();
else
dgdx->head(2) = v/v.norm();
dgdx->row(0).head(3) = v/v.norm();
}
}
};
Expand Down
1 change: 1 addition & 0 deletions lib/systems/costs/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@ set(headers
rnlqsensorcost.h
sensorcost.h
multicost.h
multilscost.h
constraintcost.h
body2dtrackcost.h
body3dtrackcost.h
Expand Down
31 changes: 27 additions & 4 deletions lib/systems/costs/constraintcost.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,13 +54,12 @@ namespace gcop {
Vectormd *Lp = 0, Matrixmd *Lpp = 0,
Matrixmnd *Lpx = 0);

/*
bool Res(Vectorgd &g,
double t, const T &x, const Vectorcd &u, double h,
const Vectormd *p = 0,
Matrixgnd *dgdx = 0, Matrixgcd *dgdu = 0,
Matrixgmd *dgdp = 0);
*/

Constraint<T, _nx, _nu, _np, _ng> &con;

double b; ///< penalty coefficient (default is 1)
Expand All @@ -79,12 +78,36 @@ namespace gcop {

if (_ng == Dynamic) {
gp.resize(con.ng);
this->ng = con.ng;
}
else
{
this->ng = _ng;
}

if (_nx == Dynamic || _ng == Dynamic) {
dgdx.resize(con.ng, sys.X.n);
}
}

template <typename T, int _nx, int _nu, int _np, int _ng>
bool ConstraintCost<T, _nx, _nu, _np, _ng>::Res(Vectorgd &g,
double t, const T &x, const Vectorcd &u, double h,
const Vectormd *p,
Matrixgnd *dgdx, Matrixgcd *dgdu,
Matrixgmd *dgdp)
{
this->con(this->g, t, x, u, p, dgdx);
for (int i = 0; i < con.ng; ++i) {
if (this->g[i] < 0) { // if constraint is satisfied then null it
this->g[i] = 0;
if (dgdx)
dgdx->row(i).setZero();
}
}
g = sqrt(b)*this->g;
return true;
}

template <typename T, int _nx, int _nu, int _np, int _ng>
double ConstraintCost<T, _nx, _nu, _np, _ng>::L(double t, const T &x, const Matrix<double, _nu, 1> &u,
Expand All @@ -97,7 +120,7 @@ namespace gcop {
Matrix<double, _np, _nx> *Lpx) {

double q = 234234023411230;
dgdx[0] = q; // random number
dgdx(0,0) = q; // random number

// only consider state constraints for now
this->con(this->g, t, x, u, p, Lx ? &dgdx : 0);
Expand All @@ -109,7 +132,7 @@ namespace gcop {
double eps = 1e-3;

// if no jacobians were provided use finite differences
if (Lx && fabs(dgdx[0] - q) < 1e-10) {
if (Lx && fabs(dgdx(0,0) - q) < 1e-10) {

for (int i = 0; i < this->sys.X.n; ++i) {
dx.setZero();
Expand Down
Loading

0 comments on commit 8d35b9a

Please sign in to comment.