From b81e9fa2c86a9bf3c876732f8adfaaf4c5bf255c Mon Sep 17 00:00:00 2001 From: "garimella.gowtham74@gmail.com" Date: Tue, 19 May 2015 20:10:49 -0400 Subject: [PATCH 1/6] Fixed some bugs in mbs systems and gndoep; Fixed naming issues with gndoepv2; Added parameter support to mbs systems --- bin/BulletVehicleEstimation/VehicleDemo.h | 4 +- bin/BulletZVehicleTest/VehicleDemo.h | 4 +- bin/body3dforceest.cc | 9 +++- lib/algos/CMakeLists.txt | 4 +- .../{gndoepv2.h => deterministicgndoep.h} | 27 ++++++---- lib/algos/gndoep.h | 8 ++- lib/systems/costs/lqsensorcost.h | 1 + lib/systems/manifolds/mbsmanifold.cc | 1 + lib/systems/manifolds/mbsstate.h | 19 +++++++ lib/systems/mbs.cc | 54 ++++++++++++------- lib/systems/mbs.h | 18 +++---- lib/systems/system.h | 8 +++ lib/systems/urdf_parser.cpp | 8 +-- lib/systems/urdf_parser.h | 7 ++- 14 files changed, 121 insertions(+), 51 deletions(-) rename lib/algos/{gndoepv2.h => deterministicgndoep.h} (91%) diff --git a/bin/BulletVehicleEstimation/VehicleDemo.h b/bin/BulletVehicleEstimation/VehicleDemo.h index 92484c32..23f37994 100644 --- a/bin/BulletVehicleEstimation/VehicleDemo.h +++ b/bin/BulletVehicleEstimation/VehicleDemo.h @@ -26,7 +26,7 @@ class btCollisionShape; #include #include "point3dgps.h" -#include "gndoepv2.h" +#include "deterministicgndoep.h" #include "params.h" #include "lqsensorcost.h" #include "utils.h" @@ -41,7 +41,7 @@ using namespace Eigen; class VehicleDemo : public GlutDemoApplication { typedef LqSensorCost RccarCost; - typedef GnDoep1 RccarDoep; + typedef GnDoep RccarDoep; protected: int iters; //Number of iterations diff --git a/bin/BulletZVehicleTest/VehicleDemo.h b/bin/BulletZVehicleTest/VehicleDemo.h index a552c0f3..f8dd3a1b 100644 --- a/bin/BulletZVehicleTest/VehicleDemo.h +++ b/bin/BulletZVehicleTest/VehicleDemo.h @@ -26,7 +26,7 @@ class btCollisionShape; #include #include "point3dgps.h" -#include "gndoepv2.h" +#include "deterministicgndoep.h" #include "params.h" #include "lqsensorcost.h" #include "utils.h" @@ -41,7 +41,7 @@ using namespace Eigen; class VehicleDemo : public GlutDemoApplication { typedef LqSensorCost RccarCost; - typedef GnDoep1 RccarDoep; + typedef GnDoep RccarDoep; protected: int iters; //Number of iterations diff --git a/bin/body3dforceest.cc b/bin/body3dforceest.cc index af78a816..4a56e176 100644 --- a/bin/body3dforceest.cc +++ b/bin/body3dforceest.cc @@ -7,10 +7,17 @@ #include "utils.h" #include "so3.h" #include "params.h" -#include "gndoep.h" #include "sensor.h" #include "insmanifold.h" +//#define USE_STOCHASTIC_DYNAMICS + +#ifdef USE_STOCHASTIC_DYNAMICS +#include "gndoep.h" +#else +#include "deterministicgndoep.h" +#endif + using namespace std; using namespace Eigen; using namespace gcop; diff --git a/lib/algos/CMakeLists.txt b/lib/algos/CMakeLists.txt index a5836266..d6f5982e 100644 --- a/lib/algos/CMakeLists.txt +++ b/lib/algos/CMakeLists.txt @@ -5,7 +5,7 @@ set(headers doep.h gndocp.h gndoep.h - gndoepv2.h + deterministicgndoep.h pddp.h ba.h body2dslam.h @@ -41,4 +41,4 @@ set(headers ${headers} body3ddemcontroller.h ) -ENDIF (DSL_FOUND AND PQP_FOUND) \ No newline at end of file +ENDIF (DSL_FOUND AND PQP_FOUND) diff --git a/lib/algos/gndoepv2.h b/lib/algos/deterministicgndoep.h similarity index 91% rename from lib/algos/gndoepv2.h rename to lib/algos/deterministicgndoep.h index 091d73c0..388244a3 100644 --- a/lib/algos/gndoepv2.h +++ b/lib/algos/deterministicgndoep.h @@ -6,6 +6,9 @@ #include +/** GnDoep with deterministic dynamics +*/ + namespace gcop { @@ -32,7 +35,7 @@ namespace gcop { int _nz = Dynamic, typename T1 = T, int _nx1 = _nx> - class GnDoep1 : public Doep { + class GnDoep : public Doep { typedef Matrix Vectorgd; typedef Matrix Matrixgxd; @@ -90,12 +93,12 @@ namespace gcop { */ - GnDoep1(System &sys, Sensor &sensor, + GnDoep(System &sys, Sensor &sensor, LqSensorCost &cost, vector &ts, vector &xs, vector &us, Vectormd &p, vector &ts1, Func_type _project=NULL, bool update = true); - virtual ~GnDoep1(); + virtual ~GnDoep(); /** * Perform one DOCP iteration. Internally calls: @@ -155,7 +158,7 @@ struct Functor struct GnCost : Functor { GnCost(int inputs, int values) : Functor(inputs, values) {}; - GnDoep1 *doep; + GnDoep *doep; typedef Matrix Vectornd; typedef Matrix Vectormd; @@ -212,9 +215,11 @@ struct Functor { std::cout<<"Input: "<p<J = 0.5*(fvec.squaredNorm());//#TODO Directly from LevinBergMarquadt if possible + //getchar(); return 0; } }; @@ -223,7 +228,7 @@ struct Functor using namespace Eigen; template - GnDoep1::GnDoep1(System &sys, Sensor &sensor, + GnDoep::GnDoep(System &sys, Sensor &sensor, LqSensorCost &cost, vector &ts, vector &xs, @@ -245,7 +250,7 @@ struct Functor } template - GnDoep1::~GnDoep1() + GnDoep::~GnDoep() { delete lm; delete numDiff; @@ -253,7 +258,7 @@ struct Functor } template - void GnDoep1::Iterate() { + void GnDoep::Iterate() { if (!lm) { functor = new GnCost(inputs, values); @@ -267,6 +272,10 @@ struct Functor const int &np = this->sys.P.n; s = this->p;//Set the system parameters to initial guess + + lm->parameters.maxfev = 1e6;//Maximum nof evaluations is very high + info = lm->minimizeInit(s); + cout <<"info="<minimize(s); + info = lm->minimizeOneStep(s); cout <<"info="< &xs, vector &us, Vectormd &p, - vector &ts1, + vector &ts1, Func_type _project, bool update) : - Doep(sys, sensor, cost, ts, xs, us, p, ts1, _project, update), + Doep(sys, sensor, cost, ts, xs, us, p, ts1, _project, false), inputs(us.size()*sys.X.n + sys.P.n), values((sys.X.n)*us.size()+(sensor.Z.n)*ts1.size()+sys.P.n), s(inputs), functor(0), numDiff(0), lm(0) { cout <<"inputs=" <Step(xb, t, xa, u, h, p, A, B, C); Matrixnd H; + if(_nx == Dynamic) + { + H.resize(this->X.n, this->X.n); + } this->NoiseMatrix(H, this->t, this->x, u, h, p); this->X.Retract(xb, xb, (H*w)); if (D) @@ -349,6 +353,10 @@ namespace gcop { if (affineNoise) { double result = this->Step(xb, u, h, p, A, B, C); Matrixnd H; + if(_nx == Dynamic) + { + H.resize(this->X.n, this->X.n); + } this->NoiseMatrix(H, this->t, this->x, u, h, p); this->X.Retract(xb, xb, (H*w)); this->x = xb;//Update internal state diff --git a/lib/systems/urdf_parser.cpp b/lib/systems/urdf_parser.cpp index 76482ebf..f5a41e13 100644 --- a/lib/systems/urdf_parser.cpp +++ b/lib/systems/urdf_parser.cpp @@ -376,7 +376,7 @@ void findnofactivejoints(boost::shared_ptr link, int &count) } } -boost::shared_ptr mbsgenerator(const string &xml_string, gcop::Matrix4d &gposei_root, string type) +boost::shared_ptr mbsgenerator(const string &xml_string, gcop::Matrix4d &gposei_root, string type, int np) { //parse the xml file into a urdf model boost::shared_ptr urdfmodel = parseURDF(xml_string); @@ -401,18 +401,18 @@ boost::shared_ptr mbsgenerator(const string &xml_string, gcop::Matrix boost::shared_ptr mbs; if(type == "AIRBASE") { - mbs.reset(new gcop::Mbs(nofactivejoints+1,4+nofactivejoints)); + mbs.reset(new gcop::Mbs(nofactivejoints+1,4+nofactivejoints, false, np)); mbs->basetype = mbs->AIRBASE; cout<<"Creating airbase "<U.n<basetype = mbs->FLOATBASE; } else if(type == "FIXEDBASE") { - mbs.reset(new gcop::Mbs(nofactivejoints+1,nofactivejoints,true)); + mbs.reset(new gcop::Mbs(nofactivejoints+1,nofactivejoints,true, np)); mbs->basetype = mbs->FIXEDBASE; } //aggregate and assign rootlink diff --git a/lib/systems/urdf_parser.h b/lib/systems/urdf_parser.h index f7e3ecd6..724b8bf0 100644 --- a/lib/systems/urdf_parser.h +++ b/lib/systems/urdf_parser.h @@ -55,7 +55,12 @@ namespace gcop_urdf{ void aggregate(boost::shared_ptr link, boost::shared_ptr parentlink,Pose cumpose); gcop::Matrix4d diffpose(Pose &posej_p,Pose &posei_p); void walkTree(boost::shared_ptr link, int level,int &index,boost::shared_ptr mbs); - boost::shared_ptr mbsgenerator(const std::string &xml_string,gcop::Matrix4d &gposei_root, std::string type = "FLOATBASE"); + /** + * Creates a Multi body system from urdf + * + * np is the number of parameters + */ + boost::shared_ptr mbsgenerator(const std::string &xml_string,gcop::Matrix4d &gposei_root, std::string type = "FLOATBASE", int np = 0); } From bdbe59a8de98628e1b181944d4ca28c1aa93a03f Mon Sep 17 00:00:00 2001 From: "garimella.gowtham74@gmail.com" Date: Thu, 21 May 2015 23:28:20 -0400 Subject: [PATCH 2/6] Fixed major bug in urdf parser corresponding to parallel axis theorem --- lib/systems/urdf_parser.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lib/systems/urdf_parser.cpp b/lib/systems/urdf_parser.cpp index f5a41e13..6236d1fb 100644 --- a/lib/systems/urdf_parser.cpp +++ b/lib/systems/urdf_parser.cpp @@ -148,7 +148,7 @@ void combineinertia(boost::shared_ptr clink, boost::shared_ptr plink //Using parallel axis theorem: double pmass = plink->inertial->mass, cmass = clink->inertial->mass; //cout<<"pmass "<()*(pmass/(cmass + pmass)); + Vector3d vecfi_pi = gposeci_pi.topRightCorner<3,1>()*(cmass/(cmass + pmass)); Vector3d vecfi_ci = vecfi_pi - gposeci_pi.topRightCorner<3,1>(); //cout<<"vecfi_pi "< link, int &count) if (*child) { boost::shared_ptr childlink = *child; - if((childlink->parent_joint->type == Joint::REVOLUTE)||(childlink->parent_joint->type == Joint::REVOLUTE)) + if((childlink->parent_joint->type == Joint::REVOLUTE)||(childlink->parent_joint->type == Joint::PRISMATIC)) count++; findnofactivejoints(*child,count); } From 957609e33e4ce32c67e863fb1ed5a802a552a4fc Mon Sep 17 00:00:00 2001 From: "garimella.gowtham74@gmail.com" Date: Mon, 25 May 2015 09:50:06 -0400 Subject: [PATCH 3/6] Added end effector name instead of children size for models with multiple end effectors --- lib/systems/mbs.cc | 18 ++++++++++++++++-- lib/systems/mbs.h | 2 ++ 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/lib/systems/mbs.cc b/lib/systems/mbs.cc index 9ee9aa80..8e8aa601 100644 --- a/lib/systems/mbs.cc +++ b/lib/systems/mbs.cc @@ -326,7 +326,7 @@ void Mbs::Bias(VectorXd &b, if(p != 0) { //External parameters provided: - if(cs[i].size() == 0) + if(links[i].name.compare(end_effector_name) == 0) { //End effector //For now support for only one end effector i.e A chain: @@ -787,7 +787,21 @@ void Mbs::DBias(VectorXd &b, se3.tlnmu(pa, -h*va, I.cwiseProduct(va)); ps[i] = pb - pa - h*gr; // ps[i] = I.cwiseProduct(vb) - I.cwiseProduct(va) - h*gr; - + if(p != 0) + { + //External parameters provided: + if(links[i].name.compare(end_effector_name) == 0) + { + //End effector + //For now support for only one end effector i.e A chain: + assert((*p).size() >= 6);//Make sure there is atleast one wrench + Vector6d external_force; + external_force.tail<3>() = (x.gs[i].topLeftCorner<3,3>().transpose()*(*p).tail<3>()); + //Torque://#VERIFY + external_force.head<3>() = (x.gs[i].topLeftCorner<3,3>().transpose()*(*p).head<3>()); + ps[i] = ps[i] - external_force; + } + } if (debug) { cout << "i=" << i << endl; cout << "va=" << va << endl; diff --git a/lib/systems/mbs.h b/lib/systems/mbs.h index 01ce7ea9..a37ef336 100644 --- a/lib/systems/mbs.h +++ b/lib/systems/mbs.h @@ -199,6 +199,8 @@ namespace gcop { static const int TRAP = 3; ///< symplectic trapezoidal 2nd order method int iters; ///< max number of Newton iterations used in symplectic method + string end_effector_name;///< Name of end effectors to which parameter forces(*p) are applied + bool debug; }; } From f6525b2540e468e38da4f5b21521532711a5eddc Mon Sep 17 00:00:00 2001 From: "garimella.gowtham74@gmail.com" Date: Mon, 25 May 2015 11:29:48 -0400 Subject: [PATCH 4/6] Added support for separating base frame from base inertial frame --- lib/systems/mbs.cc | 7 ++++++- lib/systems/mbs.h | 2 ++ lib/systems/urdf_parser.cpp | 5 +++-- lib/systems/urdf_parser.h | 2 +- 4 files changed, 12 insertions(+), 4 deletions(-) diff --git a/lib/systems/mbs.cc b/lib/systems/mbs.cc index 8e8aa601..dc6aff18 100644 --- a/lib/systems/mbs.cc +++ b/lib/systems/mbs.cc @@ -31,7 +31,8 @@ Mbs::Mbs(int nb, int c, bool fixed, int np) : nb(nb), fixed(fixed), ubK(VectorXd::Constant(nb - 1, 0.01)), ubD(VectorXd::Constant(nb - 1, 0.001)), fsl(VectorXd::Zero(nb - 1)), - fsu(VectorXd::Zero(nb - 1)) + fsu(VectorXd::Zero(nb - 1)), + pose_inertia_base(Matrix4d::Identity()) { //ag << 0, 0, -9.81; } @@ -65,6 +66,10 @@ void Mbs::Force(VectorXd &f, double t, const MbsState &x, const VectorXd &u, f[4] = 0; f[5] = u[3]; } + //Transform the base forces into inertial frame: + static Matrix6d M_inertia_base; + se3.Ad(M_inertia_base, pose_inertia_base); + f.head<6>() = M_inertia_base.transpose()*f.head<6>(); // this is ok for fixed base also f.tail(nb-1) = u.tail(nb-1) - damping.cwiseProduct(x.dr); diff --git a/lib/systems/mbs.h b/lib/systems/mbs.h index a37ef336..f7c1aa02 100644 --- a/lib/systems/mbs.h +++ b/lib/systems/mbs.h @@ -201,6 +201,8 @@ namespace gcop { string end_effector_name;///< Name of end effectors to which parameter forces(*p) are applied + Matrix4d pose_inertia_base;///< Pose of the Inertial frame of the base in the base frame. By default identity + bool debug; }; } diff --git a/lib/systems/urdf_parser.cpp b/lib/systems/urdf_parser.cpp index 6236d1fb..afe1616f 100644 --- a/lib/systems/urdf_parser.cpp +++ b/lib/systems/urdf_parser.cpp @@ -376,7 +376,7 @@ void findnofactivejoints(boost::shared_ptr link, int &count) } } -boost::shared_ptr mbsgenerator(const string &xml_string, gcop::Matrix4d &gposei_root, string type, int np) +boost::shared_ptr mbsgenerator(const string &xml_string, string type, int np) { //parse the xml file into a urdf model boost::shared_ptr urdfmodel = parseURDF(xml_string); @@ -444,7 +444,8 @@ boost::shared_ptr mbsgenerator(const string &xml_string, gcop::Matrix gcop::Vector7d posevec; //Pose ci_v = root_link->inertial->origin.GetInverse()*root_link->visual->origin; posevec<inertial->origin.rotation.w,root_link->inertial->origin.rotation.x,root_link->inertial->origin.rotation.y,root_link->inertial->origin.rotation.z,root_link->inertial->origin.position.x,root_link->inertial->origin.position.y,root_link->inertial->origin.position.z; - gcop::SE3::Instance().quatxyz2g(gposei_root,posevec); + + gcop::SE3::Instance().quatxyz2g(mbs->pose_inertia_base,posevec); cout<<"index: "< mbsgenerator(const std::string &xml_string,gcop::Matrix4d &gposei_root, std::string type = "FLOATBASE", int np = 0); + boost::shared_ptr mbsgenerator(const std::string &xml_string, std::string type = "FLOATBASE", int np = 0); } From ccffc618fddd87ee275ede9206440dd5109bec1d Mon Sep 17 00:00:00 2001 From: "garimella.gowtham74@gmail.com" Date: Mon, 6 Jul 2015 13:59:25 -0400 Subject: [PATCH 5/6] Added small changes in gn to work with mbs system --- lib/algos/gndocp.h | 18 ++++++++++--- .../parametrization/uniformsplinetparam.h | 25 +++++++++++++------ 2 files changed, 31 insertions(+), 12 deletions(-) diff --git a/lib/algos/gndocp.h b/lib/algos/gndocp.h index a93958ff..2d6abb59 100644 --- a/lib/algos/gndocp.h +++ b/lib/algos/gndocp.h @@ -100,6 +100,8 @@ using ceres::Solve; * are updated. */ void Iterate(); + + void Reset(); Tparam &tparam; int info; @@ -300,11 +302,11 @@ struct Functor inputs(tparam.ntp), values(cost.ng*xs.size()), s(inputs), functor(0), numDiff(0), lm(0), -#ifndef USE_SAMPLE_NUMERICAL_DIFF +//#ifndef USE_SAMPLE_NUMERICAL_DIFF numdiff_stepsize(1e-8) -#else - numdiff_stepsize(1e-4) -#endif +//#else + //numdiff_stepsize(1e-4) +//#endif { if(update) this->Update(false);//No need of derivatives @@ -323,6 +325,13 @@ struct Functor delete numDiff; delete functor; } + + template + void GnDocp::Reset() { + delete lm; + lm = NULL; + return; + } template void GnDocp::Iterate() { @@ -339,6 +348,7 @@ struct Functor #endif */ lm->parameters.maxfev = 1e6;//Maximum nof evaluations is very high + cout<<"Initializing..."<minimizeInit(s); cout <<"info="< class UniformSplineTparam : public Tparam { typedef Matrix Vectornd; @@ -103,9 +103,9 @@ namespace gcop { UniformSplineTparam::UniformSplineTparam(System &sys, const VectorXd &tks, int degree) : Tparam(sys, (tks.size()+degree-1)*(sys.U.n)),degree(degree), tks(tks) { assert(tks.size() >=2);//atleast 1 segments for which above constructor size is valid - cout<<"Degree: "< @@ -114,6 +114,7 @@ namespace gcop { const vector &xs, const vector &us, const Vectormd *p) { + s.resize(this->ntp); //Find control points(uks) given us int nofcontrolpoints = (degree + this->tks.size() - 1); int tks_index = 0; @@ -146,7 +147,7 @@ namespace gcop { //cout<<"A: "<sys.U.n); ind++) { for(int uind = 0; uind < us.size(); uind++) { @@ -160,7 +161,7 @@ namespace gcop { //Copy the elements back into vector s: for(int sind = 0; sind < nofcontrolpoints; sind++) { - s(sind*nu + ind) = b(sind); + s(sind*(this->sys.U.n) + ind) = b(sind); } } cout<<"s: "<tks.size() - 1)*(this->sys.U.n)); + //cout<<"s.size(): "<sys.U.n)<sys.Reset(xs[0],ts[0]); int tks_index = 0; @@ -221,7 +228,9 @@ namespace gcop { for(int degree_count = 0; degree_count <= degree; degree_count++) { //us[i] += basis[degree_count]*s.segment((tks_index+degree_count)*(this->sys.U.n), this->sys.U.n); - us[i] += basis[degree_count]*s.segment((tks_index+degree_count)*nu,nu); + //cout<<"degree_count: "<sys.U.n)<sys.U.n),(this->sys.U.n)); } // cout<<"ts["< Date: Mon, 6 Jul 2015 15:18:00 -0400 Subject: [PATCH 6/6] Added insmag --- lib/systems/sensors/CMakeLists.txt | 1 + lib/systems/sensors/insmag.h | 76 ++++++++++++++++++++++++++++++ 2 files changed, 77 insertions(+) create mode 100644 lib/systems/sensors/insmag.h diff --git a/lib/systems/sensors/CMakeLists.txt b/lib/systems/sensors/CMakeLists.txt index 39483c48..430a1304 100644 --- a/lib/systems/sensors/CMakeLists.txt +++ b/lib/systems/sensors/CMakeLists.txt @@ -2,6 +2,7 @@ set(headers imusensor.h insgps.h insimu.h + insmag.h point3dgps.h sensor.h ) diff --git a/lib/systems/sensors/insmag.h b/lib/systems/sensors/insmag.h new file mode 100644 index 00000000..877ebed1 --- /dev/null +++ b/lib/systems/sensors/insmag.h @@ -0,0 +1,76 @@ +#ifndef GCOP_INSMAG_H +#define GCOP_INSMAG_H + +#include +#include "sensor.h" +#include "ins.h" +#include "rn.h" + +namespace gcop { + + using namespace Eigen; + + /** + * General sensor model + * + * Subclasses should provide implementation for the + * sensor function () + * + * Author: Marin Kobilarov marin(at)jhu.edu + */ + template + class InsMag : public Sensor, 3> { + + public: + + typedef Matrix Vectorcd; + + typedef Matrix Vectormd; + + + typedef Matrix Vectorzd; + typedef Matrix Matrixz3d; + typedef Matrix Matrixzcd; + typedef Matrix Matrixz15d; + typedef Matrix Matrixzmd; + + InsMag(); + + bool operator()(Vectorzd &y, double t, const InsState &x, const Vectorcd &u, + const Vectormd *p = 0, + Matrixz15d *dydx = 0, Matrixzcd *dydu = 0, + Matrixzmd *dydp = 0) { + + + y.template head<3>() = x.R.transpose()*m0; + + if (dydx){ + dydx->setZero(); + Matrix3d A; + + SO3::Instance().hat(A, y.template head<3>()); + dydx->template bottomLeftCorner<3,3>() = A; + } + + return true; + } + + Vector3d m0; ///< magnetometer reference + + double srm; ///< magnetometer measurement stdev + }; + + + template + InsMag<_nu, _np>::InsMag() : + Sensor, 3>(Rn<3>::Instance()) { + + m0 << 1, 0, 0; + + srm = 0.01; ///< magnetometer measurement stdev + + this->R.template bottomRightCorner<3,3>().diagonal().setConstant(srm*srm); + } +} + +#endif