From ec7574283b88f16351982e84a924858fda143300 Mon Sep 17 00:00:00 2001 From: martimbrandao Date: Tue, 22 Nov 2016 07:25:01 +0900 Subject: [PATCH 1/9] pcl version compatibility --- src/cloudproc/cloudproc.cpp | 23 ++++++++++++++++++++--- src/cloudproc/cloudprocpy.cpp | 10 +++++++++- src/cloudproc/hacd_interface.cpp | 14 +++++++++++++- src/cloudproc/mesh_simplification.cpp | 14 +++++++++++++- 4 files changed, 55 insertions(+), 6 deletions(-) diff --git a/src/cloudproc/cloudproc.cpp b/src/cloudproc/cloudproc.cpp index 699831d3..9e4d9d71 100644 --- a/src/cloudproc/cloudproc.cpp +++ b/src/cloudproc/cloudproc.cpp @@ -17,7 +17,7 @@ #include "pcl/impl/instantiate.hpp" #include #include -#if PCL_MINOR_VERSION > 6 +#if PCL_VERSION_COMPARE(>=, 1, 7, 0) #include #include #endif @@ -43,10 +43,18 @@ void setWidthToSize(const CloudT& cloud) { template typename pcl::PointCloud::Ptr readPCD(const std::string& pcdfile) { +#if PCL_VERSION_COMPARE(>=, 1, 7, 0) + pcl::PCLPointCloud2 cloud_blob; +#else sensor_msgs::PointCloud2 cloud_blob; +#endif typename pcl::PointCloud::Ptr cloud (new typename pcl::PointCloud); if (pcl::io::loadPCDFile (pcdfile, cloud_blob) != 0) FILE_OPEN_ERROR(pcdfile); +#if PCL_VERSION_COMPARE(>=, 1, 7, 0) + pcl::fromPCLPointCloud2 (cloud_blob, *cloud); +#else pcl::fromROSMsg (cloud_blob, *cloud); +#endif return cloud; } @@ -194,7 +202,11 @@ pcl::PolygonMesh::Ptr meshOFM(PointCloud::ConstPtr cloud, int edg ofm.setTriangulationType (pcl::OrganizedFastMesh::TRIANGLE_ADAPTIVE_CUT); pcl::PolygonMeshPtr mesh(new pcl::PolygonMesh()); ofm.reconstruct(mesh->polygons); +#if PCL_VERSION_COMPARE(>=, 1, 7, 0) + pcl::toPCLPointCloud2(*cloud, mesh->cloud); +#else pcl::toROSMsg(*cloud, mesh->cloud); +#endif mesh->header = cloud->header; return mesh; } @@ -287,7 +299,7 @@ typename pcl::PointCloud::Ptr maskFilter(typename pcl::PointCloud::ConstPt template typename pcl::PointCloud::Ptr medianFilter(typename pcl::PointCloud::ConstPtr in, int windowSize, float maxAllowedMovement) { -#if PCL_MINOR_VERSION > 6 +#if PCL_VERSION_COMPARE(>=, 1, 7, 0) pcl::MedianFilter mf; mf.setWindowSize(windowSize); mf.setMaxAllowedMovement(maxAllowedMovement); @@ -302,7 +314,7 @@ typename pcl::PointCloud::Ptr medianFilter(typename pcl::PointCloud::Const template typename pcl::PointCloud::Ptr fastBilateralFilter(typename pcl::PointCloud::ConstPtr in, float sigmaS, float sigmaR) { -#if PCL_MINOR_VERSION > 6 +#if PCL_VERSION_COMPARE(>=, 1, 7, 0) pcl::FastBilateralFilter mf; mf.setSigmaS(sigmaS); mf.setSigmaR(sigmaR); @@ -315,8 +327,13 @@ typename pcl::PointCloud::Ptr fastBilateralFilter(typename pcl::PointCloud #endif } +#if PCL_VERSION_COMPARE(>=, 1, 7, 0) +void removenans(pcl::PCLPointCloud2& cloud, float fillval=0); +void removenans(pcl::PCLPointCloud2& cloud, float fillval) { +#else void removenans(sensor_msgs::PointCloud2& cloud, float fillval=0); void removenans(sensor_msgs::PointCloud2& cloud, float fillval) { +#endif int npts = cloud.width * cloud.height; for (int i=0; i < npts; ++i) { float* ptdata = (float*)(cloud.data.data() + cloud.point_step * i); diff --git a/src/cloudproc/cloudprocpy.cpp b/src/cloudproc/cloudprocpy.cpp index 2208b92f..65d73355 100644 --- a/src/cloudproc/cloudprocpy.cpp +++ b/src/cloudproc/cloudprocpy.cpp @@ -7,10 +7,14 @@ #include "utils/stl_to_string.hpp" #include "cloudgrabber.hpp" #include "cloudproc/mesh_simplification.hpp" -#include #include #include #include "hacd_interface.hpp" +#if PCL_VERSION_COMPARE(>=, 1, 7, 0) +#include +#else +#include +#endif using namespace Eigen; using namespace pcl; @@ -100,7 +104,11 @@ struct PyCloud { CloudXYZ::Ptr PolygonMesh_getCloud(const PolygonMesh* mesh) { CloudXYZ::Ptr cloud(new CloudXYZ()); +#if PCL_VERSION_COMPARE(>=, 1, 7, 0) + pcl::fromPCLPointCloud2(mesh->cloud, *cloud); +#else pcl::fromROSMsg(mesh->cloud, *cloud); +#endif return cloud; } py::object PolygonMesh_getVertices(const PolygonMesh* mesh) { diff --git a/src/cloudproc/hacd_interface.cpp b/src/cloudproc/hacd_interface.cpp index d96fb6f7..6aaa2b5f 100644 --- a/src/cloudproc/hacd_interface.cpp +++ b/src/cloudproc/hacd_interface.cpp @@ -2,8 +2,12 @@ #include #include #include -#include #include +#if PCL_VERSION_COMPARE(>=, 1, 7, 0) +#include +#else +#include +#endif using HACD::Vec3; using HACD::Real; using namespace pcl; @@ -16,7 +20,11 @@ void polygonMeshFromPointsTriangles(pcl::PolygonMesh& mesh, const vector< Vec3=, 1, 7, 0) + pcl::toPCLPointCloud2(cloud, mesh.cloud); +#else pcl::toROSMsg(cloud, mesh.cloud); +#endif mesh.polygons.resize(triangles.size()); for (int i=0; i < triangles.size(); ++i) { vector& vertices = mesh.polygons[i].vertices; @@ -28,7 +36,11 @@ void polygonMeshFromPointsTriangles(pcl::PolygonMesh& mesh, const vector< Vec3 >& points, vector< Vec3 >& triangles) { PointCloud cloud; +#if PCL_VERSION_COMPARE(>=, 1, 7, 0) + pcl::fromPCLPointCloud2 (mesh.cloud, cloud); +#else pcl::fromROSMsg (mesh.cloud, cloud); +#endif points.resize(cloud.size()); for (int i=0; i < cloud.size(); ++i) { points[i].X() = cloud.points[i].x; diff --git a/src/cloudproc/mesh_simplification.cpp b/src/cloudproc/mesh_simplification.cpp index a7bf66a5..3d60d4e8 100644 --- a/src/cloudproc/mesh_simplification.cpp +++ b/src/cloudproc/mesh_simplification.cpp @@ -9,8 +9,12 @@ #include #include #include -#include #include "macros.h" +#if PCL_VERSION_COMPARE(>=, 1, 7, 0) +#include +#else +#include +#endif using namespace pcl; namespace { @@ -43,12 +47,20 @@ void toPCL(vtkPolyData& in, pcl::PolygonMesh& out) { vertices[1] = idxdata[1]; vertices[2] = idxdata[2]; } +#if PCL_VERSION_COMPARE(>=, 1, 7, 0) + pcl::toPCLPointCloud2(cloud, out.cloud); +#else pcl::toROSMsg(cloud, out.cloud); +#endif } void toVTK(pcl::PolygonMesh& in, vtkPolyData& out) { pcl::PointCloud cloud; +#if PCL_VERSION_COMPARE(>=, 1, 7, 0) + pcl::fromPCLPointCloud2(in.cloud, cloud); +#else pcl::fromROSMsg(in.cloud, cloud); +#endif vtkSmartPointer points = vtkSmartPointer::New(); BOOST_FOREACH(const PointXYZ& pt, cloud.points) { From a169938aaee0e3ffb00782d6bf9e9f14bdd20988 Mon Sep 17 00:00:00 2001 From: martimbrandao Date: Tue, 6 Dec 2016 15:18:05 +0900 Subject: [PATCH 2/9] missing pragma in header --- src/trajopt/problem_description.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/trajopt/problem_description.hpp b/src/trajopt/problem_description.hpp index 94001577..67877901 100644 --- a/src/trajopt/problem_description.hpp +++ b/src/trajopt/problem_description.hpp @@ -1,3 +1,4 @@ +#pragma once #include "trajopt/common.hpp" #include "json_marshal.hpp" #include From cf5107fda8ceebe5a1cd962bd53487a674049f48 Mon Sep 17 00:00:00 2001 From: martimbrandao Date: Tue, 6 Dec 2016 15:19:15 +0900 Subject: [PATCH 3/9] viewer screenshots and background color --- src/osgviewer/osgviewer.cpp | 58 +++++++++++++++++++++++++++++++++++++ src/osgviewer/osgviewer.hpp | 5 ++-- 2 files changed, 61 insertions(+), 2 deletions(-) diff --git a/src/osgviewer/osgviewer.cpp b/src/osgviewer/osgviewer.cpp index b72ca1bc..6aa0dfd3 100644 --- a/src/osgviewer/osgviewer.cpp +++ b/src/osgviewer/osgviewer.cpp @@ -14,6 +14,8 @@ #include #include #include +#include +#include #include #include "utils/logging.hpp" #include "openrave_userdata_utils.hpp" @@ -232,6 +234,33 @@ void AddLights(osg::Group* group) { } +// http://forum.openscenegraph.org/viewtopic.php?t=7214 +class SnapImageDrawCallback : public osg::CameraNode::DrawCallback { +public: + SnapImageDrawCallback() { _snapImageOnNextFrame = false; } + void setFileName(const std::string& filename) { _filename = filename; } + const std::string& getFileName() const { return _filename; } + void setSnapImageOnNextFrame(bool flag) { _snapImageOnNextFrame = flag; } + bool getSnapImageOnNextFrame() const { return _snapImageOnNextFrame; } + virtual void operator () (const osg::CameraNode& camera) const { + if (!_snapImageOnNextFrame) return; + int x,y,width,height; + x = camera.getViewport()->x(); + y = camera.getViewport()->y(); + width = camera.getViewport()->width(); + height = camera.getViewport()->height(); + osg::ref_ptr image = new osg::Image; + image->readPixels(x,y,width,height,GL_RGB,GL_UNSIGNED_BYTE); + if (osgDB::writeImageFile(*image,_filename)) + std::cout << "Saved screenshot to `"<<_filename<<"`"<< std::endl; + _snapImageOnNextFrame = false; + } +protected: + std::string _filename; + mutable bool _snapImageOnNextFrame; +}; + + // http://forum.openscenegraph.org/viewtopic.php?t=7806 void AddCylinderBetweenPoints(const osg::Vec3& StartPoint, osg::Vec3 EndPoint, float radius, const osg::Vec4& CylinderColor, osg::Group *pAddToThisGroup, bool use_cone) { @@ -461,8 +490,12 @@ OSGViewer::OSGViewer(EnvironmentBasePtr env) : ViewerBase(env), m_idling(false) AddLights(m_root); m_cam->setClearColor(osg::Vec4(1,1,1,1)); + osg::ref_ptr snapImageDrawCallback = new SnapImageDrawCallback(); + m_cam->setPostDrawCallback (snapImageDrawCallback.get()); + AddKeyCallback('h', boost::bind(&OSGViewer::PrintHelp, this), "Display help"); AddKeyCallback('p', boost::bind(&OSGViewer::Idle, this), "Toggle idle"); + AddKeyCallback('s', boost::bind(&OSGViewer::TakeScreenshot, this), "Take screenshot"); AddKeyCallback(osgGA::GUIEventAdapter::KEY_Escape, &throw_runtime_error, "Quit (raise exception)"); PrintHelp(); m_viewer.setRunFrameScheme(osgViewer::ViewerBase::ON_DEMAND); @@ -528,6 +561,31 @@ void OSGViewer::UpdateSceneData() { m_viewer.requestRedraw(); } +void OSGViewer::SetBkgndColor(const RaveVectorf& color) { + m_cam->setClearColor(toOsgVec4(color)); +} + +void OSGViewer::TakeScreenshot(const std::string& filename) { + osg::ref_ptr snapImageDrawCallback = dynamic_cast (m_cam->getPostDrawCallback()); + if(snapImageDrawCallback.get()) { + snapImageDrawCallback->setFileName(filename); + snapImageDrawCallback->setSnapImageOnNextFrame(true); + } else { + std::cout << "Warning: could not take screenshot" << std::endl; + } +} + +void OSGViewer::TakeScreenshot() { + time_t rawtime; + struct tm * timeinfo; + char buffer[80]; + time (&rawtime); + timeinfo = localtime(&rawtime); + strftime(buffer,80,"%Y%m%d-%I%M%S.png",timeinfo); + std::string filename(buffer); + TakeScreenshot(filename); +} + void OSGViewer::AddMouseCallback(const MouseCallback& cb) { m_handler->event_cbs.push_back(cb); // todo: print help diff --git a/src/osgviewer/osgviewer.hpp b/src/osgviewer/osgviewer.hpp index 3aa23613..a4795ffe 100644 --- a/src/osgviewer/osgviewer.hpp +++ b/src/osgviewer/osgviewer.hpp @@ -23,8 +23,9 @@ class TRAJOPT_API OSGViewer : public OpenRAVE::ViewerBase { void Idle(); // should be called ToggleIdle void UpdateSceneData(); const std::string& GetName() const {return m_name;} - void SetBkgndColor(const RaveVectorf &) {printf("warning: SetBkgndColor not implemented\n");} - + void SetBkgndColor(const RaveVectorf& color); + void TakeScreenshot(const std::string& filename); + void TakeScreenshot(); // return false if you want to disable the default TrackballManipulator handling typedef boost::function MouseCallback; From 939e36615028930b625f637257034a8addfe1907 Mon Sep 17 00:00:00 2001 From: martimbrandao Date: Tue, 6 Dec 2016 15:20:28 +0900 Subject: [PATCH 4/9] interface to get vars associated with cost/constraint --- src/sco/modeling.hpp | 4 ++++ src/sco/modeling_utils.hpp | 3 +++ src/trajopt/collision_terms.hpp | 2 ++ 3 files changed, 9 insertions(+) diff --git a/src/sco/modeling.hpp b/src/sco/modeling.hpp index 8de6985d..e303f8d2 100644 --- a/src/sco/modeling.hpp +++ b/src/sco/modeling.hpp @@ -97,6 +97,8 @@ class Cost { virtual double value(const vector&) = 0; /** Convexify at solution vector x*/ virtual ConvexObjectivePtr convex(const vector& x, Model* model) = 0; + /** Get problem variables associated with this cost */ + virtual VarVector getVars() {return VarVector();} string name() {return name_;} void setName(const string& name) {name_=name;} @@ -123,6 +125,8 @@ class Constraint { vector violations(const vector& x); /** Sum of violations */ double violation(const vector& x); + /** Get problem variables associated with this constraint */ + virtual VarVector getVars() {return VarVector();} string name() {return name_;} void setName(const string& name) {name_=name;} diff --git a/src/sco/modeling_utils.hpp b/src/sco/modeling_utils.hpp index 5f019a82..87ba9de3 100644 --- a/src/sco/modeling_utils.hpp +++ b/src/sco/modeling_utils.hpp @@ -42,6 +42,7 @@ class CostFromFunc : public Cost { CostFromFunc(ScalarOfVectorPtr f, const VarVector& vars, const string& name, bool full_hessian=false); double value(const vector& x); ConvexObjectivePtr convex(const vector& x, Model* model); + VarVector getVars() {return vars_;} protected: ScalarOfVectorPtr f_; VarVector vars_; @@ -57,6 +58,7 @@ class CostFromErrFunc : public Cost { CostFromErrFunc(VectorOfVectorPtr f, MatrixOfVectorPtr dfdx, const VarVector& vars, const VectorXd& coeffs, PenaltyType pen_type, const string& name); double value(const vector& x); ConvexObjectivePtr convex(const vector& x, Model* model); + VarVector getVars() {return vars_;} protected: VectorOfVectorPtr f_; MatrixOfVectorPtr dfdx_; @@ -75,6 +77,7 @@ class ConstraintFromFunc : public Constraint { vector value(const vector& x); ConvexConstraintsPtr convex(const vector& x, Model* model); ConstraintType type() {return type_;} + VarVector getVars() {return vars_;} protected: VectorOfVectorPtr f_; MatrixOfVectorPtr dfdx_; diff --git a/src/trajopt/collision_terms.hpp b/src/trajopt/collision_terms.hpp index e3dea618..668c004b 100644 --- a/src/trajopt/collision_terms.hpp +++ b/src/trajopt/collision_terms.hpp @@ -81,6 +81,7 @@ class TRAJOPT_API CollisionCost : public Cost, public Plotter { virtual ConvexObjectivePtr convex(const vector& x, Model* model); virtual double value(const vector&); void Plot(const DblVec& x, OR::EnvironmentBase& env, std::vector& handles); + VarVector getVars() {return m_calc->GetVars();} private: CollisionEvaluatorPtr m_calc; double m_dist_pen; @@ -95,6 +96,7 @@ class TRAJOPT_API CollisionConstraint : public IneqConstraint { virtual ConvexConstraintsPtr convex(const vector& x, Model* model); virtual DblVec value(const vector&); void Plot(const DblVec& x, OR::EnvironmentBase& env, std::vector& handles); + VarVector getVars() {return m_calc->GetVars();} private: CollisionEvaluatorPtr m_calc; double m_dist_pen; From 08e5219245b0a22dd6c108eb10e28c19763ea443 Mon Sep 17 00:00:00 2001 From: martimbrandao Date: Wed, 14 Dec 2016 16:15:32 +0900 Subject: [PATCH 5/9] interface to get viewer image --- src/osgviewer/osgviewer.cpp | 17 ++++++++++++++--- src/osgviewer/osgviewer.hpp | 1 + 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/src/osgviewer/osgviewer.cpp b/src/osgviewer/osgviewer.cpp index 6aa0dfd3..362955ea 100644 --- a/src/osgviewer/osgviewer.cpp +++ b/src/osgviewer/osgviewer.cpp @@ -249,15 +249,17 @@ class SnapImageDrawCallback : public osg::CameraNode::DrawCallback { y = camera.getViewport()->y(); width = camera.getViewport()->width(); height = camera.getViewport()->height(); - osg::ref_ptr image = new osg::Image; - image->readPixels(x,y,width,height,GL_RGB,GL_UNSIGNED_BYTE); - if (osgDB::writeImageFile(*image,_filename)) + _image = new osg::Image; + _image->readPixels(x,y,width,height,GL_RGB,GL_UNSIGNED_BYTE); + if (_filename != "" && osgDB::writeImageFile(*_image,_filename)) std::cout << "Saved screenshot to `"<<_filename<<"`"<< std::endl; _snapImageOnNextFrame = false; } + osg::ref_ptr getImage() const { return _image; } protected: std::string _filename; mutable bool _snapImageOnNextFrame; + mutable osg::ref_ptr _image; }; @@ -586,6 +588,15 @@ void OSGViewer::TakeScreenshot() { TakeScreenshot(filename); } +osg::ref_ptr OSGViewer::GetLastScreenshot() +{ + osg::ref_ptr snapImageDrawCallback = dynamic_cast (m_cam->getPostDrawCallback()); + if(snapImageDrawCallback.get()) + return snapImageDrawCallback->getImage(); + else + return osg::ref_ptr(); +} + void OSGViewer::AddMouseCallback(const MouseCallback& cb) { m_handler->event_cbs.push_back(cb); // todo: print help diff --git a/src/osgviewer/osgviewer.hpp b/src/osgviewer/osgviewer.hpp index a4795ffe..4a6d43ae 100644 --- a/src/osgviewer/osgviewer.hpp +++ b/src/osgviewer/osgviewer.hpp @@ -26,6 +26,7 @@ class TRAJOPT_API OSGViewer : public OpenRAVE::ViewerBase { void SetBkgndColor(const RaveVectorf& color); void TakeScreenshot(const std::string& filename); void TakeScreenshot(); + osg::ref_ptr GetLastScreenshot(); // return false if you want to disable the default TrackballManipulator handling typedef boost::function MouseCallback; From ed53b9c942d8fe9cbb5ba0055b8d5b1cab08e92b Mon Sep 17 00:00:00 2001 From: martimbrandao Date: Thu, 15 Dec 2016 15:50:37 +0900 Subject: [PATCH 6/9] fixed bug on viewer image interface (need to copy pixel data) --- src/osgviewer/osgviewer.cpp | 33 +++++++++++++++++++++++---------- src/osgviewer/osgviewer.hpp | 2 +- 2 files changed, 24 insertions(+), 11 deletions(-) diff --git a/src/osgviewer/osgviewer.cpp b/src/osgviewer/osgviewer.cpp index 362955ea..83e10a35 100644 --- a/src/osgviewer/osgviewer.cpp +++ b/src/osgviewer/osgviewer.cpp @@ -249,17 +249,28 @@ class SnapImageDrawCallback : public osg::CameraNode::DrawCallback { y = camera.getViewport()->y(); width = camera.getViewport()->width(); height = camera.getViewport()->height(); - _image = new osg::Image; - _image->readPixels(x,y,width,height,GL_RGB,GL_UNSIGNED_BYTE); - if (_filename != "" && osgDB::writeImageFile(*_image,_filename)) + osg::ref_ptr image = new osg::Image; + image->readPixels(x,y,width,height,GL_RGB,GL_UNSIGNED_BYTE); + // make a local copy + unsigned char * p = image->data(); + unsigned int numBytes = image->computeNumComponents(image->getPixelFormat()); + _image = std::vector(p, p + height*width*numBytes / sizeof(unsigned char)); + _height = height; + _width = width; + // save file + if (_filename != "" && osgDB::writeImageFile(*image,_filename)) std::cout << "Saved screenshot to `"<<_filename<<"`"<< std::endl; _snapImageOnNextFrame = false; } - osg::ref_ptr getImage() const { return _image; } + std::vector getImage() const { return _image; } + unsigned int getHeight() const { return _height; } + unsigned int getWidth() const { return _width; } protected: std::string _filename; mutable bool _snapImageOnNextFrame; - mutable osg::ref_ptr _image; + mutable std::vector _image; + mutable unsigned int _height; + mutable unsigned int _width; }; @@ -588,13 +599,15 @@ void OSGViewer::TakeScreenshot() { TakeScreenshot(filename); } -osg::ref_ptr OSGViewer::GetLastScreenshot() +bool OSGViewer::GetLastScreenshot(std::vector& image, unsigned int& height, unsigned int& width) { osg::ref_ptr snapImageDrawCallback = dynamic_cast (m_cam->getPostDrawCallback()); - if(snapImageDrawCallback.get()) - return snapImageDrawCallback->getImage(); - else - return osg::ref_ptr(); + if(!snapImageDrawCallback.get()) + return false; + image = snapImageDrawCallback->getImage(); + height = snapImageDrawCallback->getHeight(); + width = snapImageDrawCallback->getWidth(); + return true; } void OSGViewer::AddMouseCallback(const MouseCallback& cb) { diff --git a/src/osgviewer/osgviewer.hpp b/src/osgviewer/osgviewer.hpp index 4a6d43ae..a62d324d 100644 --- a/src/osgviewer/osgviewer.hpp +++ b/src/osgviewer/osgviewer.hpp @@ -26,7 +26,7 @@ class TRAJOPT_API OSGViewer : public OpenRAVE::ViewerBase { void SetBkgndColor(const RaveVectorf& color); void TakeScreenshot(const std::string& filename); void TakeScreenshot(); - osg::ref_ptr GetLastScreenshot(); + bool GetLastScreenshot(std::vector& image, unsigned int& height, unsigned int& width); // return false if you want to disable the default TrackballManipulator handling typedef boost::function MouseCallback; From 5d21ec1ac03993c80226f98b9fc667046b201b63 Mon Sep 17 00:00:00 2001 From: martimbrandao Date: Thu, 26 Jan 2017 17:35:56 +0900 Subject: [PATCH 7/9] missing break in switch (bullet) --- src/trajopt/bullet_collision_checker.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/trajopt/bullet_collision_checker.cpp b/src/trajopt/bullet_collision_checker.cpp index 288c0575..1050f035 100644 --- a/src/trajopt/bullet_collision_checker.cpp +++ b/src/trajopt/bullet_collision_checker.cpp @@ -202,6 +202,7 @@ btCollisionShape* createShapePrimitive(OR::KinBody::Link::GeometryPtr geom, bool } } + break; } default: assert(0 && "unrecognized collision shape type"); From bf823b194cded8b29d20ead3e3e7fb71177a4032 Mon Sep 17 00:00:00 2001 From: martimbrandao Date: Wed, 1 Mar 2017 16:52:43 +0900 Subject: [PATCH 8/9] added raycast checking with bullet --- src/trajopt/bullet_collision_checker.cpp | 9 +++++++++ src/trajopt/collision_checker.hpp | 3 +++ 2 files changed, 12 insertions(+) diff --git a/src/trajopt/bullet_collision_checker.cpp b/src/trajopt/bullet_collision_checker.cpp index 1050f035..d821f20d 100644 --- a/src/trajopt/bullet_collision_checker.cpp +++ b/src/trajopt/bullet_collision_checker.cpp @@ -349,6 +349,15 @@ class BulletCollisionChecker : public CollisionChecker { COW *cow0 = GetCow(&link0), *cow1 = GetCow(&link1); if (cow0 && cow1) m_allowedCollisionMatrix(cow0->m_index, cow1->m_index) = 1; } + virtual bool RayCastCollision(const OpenRAVE::Vector& point1, const OpenRAVE::Vector& point2) { + btVector3 btFrom = toBt(point1); + btVector3 btTo = toBt(point2); + btCollisionWorld::ClosestRayResultCallback res(btFrom, btTo); + UpdateBulletFromRave(); + m_world->updateAabbs(); + m_world->rayTest(btFrom, btTo, res); + return res.hasHit(); + } // collision checking virtual void AllVsAll(vector& collisions); virtual void LinksVsAll(const vector& links, vector& collisions, short filterMask); diff --git a/src/trajopt/collision_checker.hpp b/src/trajopt/collision_checker.hpp index 91881492..ee420f21 100644 --- a/src/trajopt/collision_checker.hpp +++ b/src/trajopt/collision_checker.hpp @@ -69,6 +69,9 @@ class TRAJOPT_API CollisionChecker : public OR::UserData { virtual void ExcludeCollisionPair(const KinBody::Link& link0, const KinBody::Link& link1) = 0; virtual void IncludeCollisionPair(const KinBody::Link& link0, const KinBody::Link& link1) = 0; + /** Check whether a raycast hits the environment */ + virtual bool RayCastCollision(const OpenRAVE::Vector& point1, const OpenRAVE::Vector& point2) = 0; + OpenRAVE::EnvironmentBaseConstPtr GetEnv() {return m_env;} From 8da5bb00b690647318a12fa4f5dd7e6c8a0f501c Mon Sep 17 00:00:00 2001 From: martimbrandao Date: Tue, 23 May 2017 23:35:18 +0900 Subject: [PATCH 9/9] save gurobi's Irreducible Infeasible Sets (IIS) for easier debug of infeasible optimization problems --- src/sco/gurobi_interface.cpp | 5 ++++- src/sco/optimizers.cpp | 3 ++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/sco/gurobi_interface.cpp b/src/sco/gurobi_interface.cpp index 69140f76..1d438475 100644 --- a/src/sco/gurobi_interface.cpp +++ b/src/sco/gurobi_interface.cpp @@ -188,7 +188,10 @@ CvxOptStatus GurobiModel::optimize(){ LOG_DEBUG("solver objective value: %.3e", objval); return CVX_SOLVED; } - else if (status == GRB_INFEASIBLE) return CVX_INFEASIBLE; + else if (status == GRB_INFEASIBLE) { + GRBcomputeIIS(m_model); + return CVX_INFEASIBLE; + } else return CVX_FAILED; } CvxOptStatus GurobiModel::optimizeFeasRelax(){ diff --git a/src/sco/optimizers.cpp b/src/sco/optimizers.cpp index 8e37ba89..9f1000bf 100644 --- a/src/sco/optimizers.cpp +++ b/src/sco/optimizers.cpp @@ -289,8 +289,9 @@ OptStatus BasicTrustRegionSQP::optimize() { CvxOptStatus status = model_->optimize(); ++results_.n_qp_solves; if (status != CVX_SOLVED) { - LOG_ERROR("convex solver failed! set TRAJOPT_LOG_THRESH=DEBUG to see solver output. saving model to /tmp/fail.lp"); + LOG_ERROR("convex solver failed! set TRAJOPT_LOG_THRESH=DEBUG to see solver output. saving model to /tmp/fail.lp and IIS to /tmp/fail.ilp"); model_->writeToFile("/tmp/fail.lp"); + model_->writeToFile("/tmp/fail.ilp"); retval = OPT_FAILED; goto cleanup; }