Merge remote-tracking branch 'origin/production' into c++17

CMakeLists.txt

@@ -4,8 +4,8 @@ set( CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE )
 
 # Define here the needed parameters
 set (OPENRAVE_VERSION_MAJOR 0)
-set (OPENRAVE_VERSION_MINOR 148)
-set (OPENRAVE_VERSION_PATCH 2)
+set (OPENRAVE_VERSION_MINOR 149)
+set (OPENRAVE_VERSION_PATCH 0)
 set (OPENRAVE_VERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR}.${OPENRAVE_VERSION_PATCH})
 set (OPENRAVE_SOVERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR})
 message(STATUS "Compiling OpenRAVE Version ${OPENRAVE_VERSION}, soversion=${OPENRAVE_SOVERSION}")

docs/source/changelog.rst

@@ -3,6 +3,11 @@
 ChangeLog
 #########
 
+Version 0.149.0
+===============
+
+- Add new geometry types "Prism" and "Capsule"
+
 Version 0.148.1
 ===============
 

include/openrave/kinbody.h

@@ -55,6 +55,8 @@ enum GeometryType : uint8_t
     GT_CalibrationBoard=7, ///< a box shaped geometry with grid of cylindrical dots of two sizes. The dots are always on the +z side of the box and are oriented towards z-axis.
     GT_Axial = 8, ///< a geometry defined by many slices along an axis, oriented towards z-axis
     GT_ConicalFrustum = 9, ///< a geometry defined by a conical frustum, oriented towards z-axis
+    GT_Prism = 10, ///< Non-trimesh right prisms with arbitrary non-convex cross-section and infinite height, which represent “walls” or “safe zones”. _meshcollision.vertices[2 * i].xy describes the cross-section.
+    GT_Capsule = 11, ///< Non-trimesh capsules (oriented towards z-axis), representing "robot link model" or "gripper model", which are able of degenerating to perfect spheres with the height tends to zero.
 };
 
 OPENRAVE_API const char* GetGeometryTypeString(GeometryType geometryType);
@@ -423,6 +425,15 @@ class OPENRAVE_API KinBody : public InterfaceBase
         inline dReal GetConicalFrustumHeight() const {
             return _vGeomData.z;
         }
+        inline dReal GetPrismHeight() const {
+            return _vGeomData.y;
+        }
+        inline dReal GetCapsuleRadius() const {
+            return _vGeomData.x;
+        }
+        inline dReal GetCapsuleHeight() const {
+            return _vGeomData.y;
+        }
         inline const Vector& GetBoxExtents() const { // deprecated?
             return _vGeomData;
         }
@@ -743,6 +754,15 @@ class OPENRAVE_API KinBody : public InterfaceBase
         inline dReal GetConicalFrustumHeight() const {
             return _info.GetConicalFrustumHeight();
         }
+        inline dReal GetPrismHeight() const {
+            return _info.GetPrismHeight();
+        }
+        inline dReal GetCapsuleRadius() const {
+            return _info.GetCapsuleRadius();
+        }
+        inline dReal GetCapsuleHeight() const {
+            return _info.GetCapsuleHeight();
+        }
         inline const Vector& GetBoxExtents() const {
             return _info.GetBoxExtents();
         }

include/openrave/openrave.h

@@ -637,10 +637,14 @@ class OPENRAVE_API StringReadable : public Readable
 {
 public:
     StringReadable(const std::string& id, const std::string& data);
+    StringReadable(const std::string& id, std::string&& data);
+    StringReadable(const std::string& id, const char* data, size_t dataLength);
     virtual ~StringReadable();
 
     /// \brief sets new string data
     void SetData(const std::string& newdata);
+    void SetData(std::string&& newdata);
+    void SetData(const char* data, size_t dataLength);
 
     /// \brief gets a reference to the saved data;
     const std::string& GetData() const;

plugins/bulletrave/bulletspace.h

@@ -178,6 +178,10 @@ class BulletSpace : public boost::enable_shared_from_this<BulletSpace>
                     // cylinder axis aligned to Y
                     child.reset(new btCylinderShapeZ(btVector3(geom->GetCylinderRadius(),geom->GetCylinderRadius(),geom->GetCylinderHeight()*0.5f)));
                     break;
+                case GT_Capsule:
+                    child.reset(new btCapsuleShapeZ(geom->GetCapsuleRadius(), geom->GetCapsuleHeight());
+                    break;
+                case GT_Prism:
                 case GT_ConicalFrustum:
                 case GT_Axial:
                 case GT_TriMesh: {

plugins/fclrave/fclspace.cpp

@@ -464,6 +464,14 @@ void _AppendFclBoxCollsionObject(const OpenRAVE::Vector& fullExtents, const Open
     contents.emplace_back(std::make_shared<fcl::CollisionObject>(fclGeom, fclTrans));
 }
 
+/// \brief helper function to initialize fcl::Container
+void _AppendFclHalfspaceCollsionObject(const OpenRAVE::Transform& trans, std::vector<std::shared_ptr<fcl::CollisionObject> >& contents)
+{
+    std::shared_ptr<fcl::CollisionGeometry> fclGeom = std::make_shared<fcl::Halfspace>(fcl::Vec3f(0, 0, 1), 0);
+    const fcl::Transform3f fclTrans(ConvertQuaternionToFCL(trans.rot), ConvertVectorToFCL(trans.trans));
+    contents.emplace_back(std::make_shared<fcl::CollisionObject>(fclGeom, fclTrans));
+}
+
 CollisionGeometryPtr FCLSpace::_CreateFCLGeomFromGeometryInfo(const KinBody::GeometryInfo &info)
 {
     switch(info._type) {
@@ -481,6 +489,9 @@ CollisionGeometryPtr FCLSpace::_CreateFCLGeomFromGeometryInfo(const KinBody::Geo
     case OpenRAVE::GT_Cylinder:
         return std::make_shared<fcl::Cylinder>(info._vGeomData.x, info._vGeomData.y);
 
+    case OpenRAVE::GT_Capsule:
+        return std::make_shared<fcl::Capsule>(info._vGeomData.x, info._vGeomData.y);
+
     case OpenRAVE::GT_Container:
     {
         const Vector& outerextents = info._vGeomData;
@@ -537,6 +548,25 @@ CollisionGeometryPtr FCLSpace::_CreateFCLGeomFromGeometryInfo(const KinBody::Geo
         _AppendFclBoxCollsionObject(2.0*vCageBaseExtents, Vector(0, 0, vCageBaseExtents.z), contents);
         return std::make_shared<fcl::Container>(contents);
     }
+    case OpenRAVE::GT_Prism:
+    {
+        std::vector<std::shared_ptr<fcl::CollisionObject> > contents;
+        const OpenRAVE::TriMesh& mesh = info._meshcollision;
+        const size_t nPoints = mesh.vertices.size();
+        for( size_t ipoint = 0; ipoint < nPoints; ipoint += 2 ) {
+            const OpenRAVE::Vector p0(mesh.vertices[ipoint].x, mesh.vertices[ipoint].y, 0);
+            const OpenRAVE::Vector p1(mesh.vertices[(ipoint + 2) % nPoints].x, mesh.vertices[(ipoint + 2) % nPoints].y, 0);
+            if( (p1 - p0).lengthsqr2() < g_fEpsilon ) {
+                continue; // ipoint
+            }
+            OpenRAVE::Transform trans(OpenRAVE::geometry::quatRotateDirection(OpenRAVE::Vector(1, 0, 0), (p1 - p0).normalize()), (p0 + p1) * 0.5); // Y pointing to the left side of the directed segment (p0, p1)
+            trans *= OpenRAVE::Transform(OpenRAVE::geometry::quatFromAxisAngle(OpenRAVE::Vector(1, 0, 0), -M_PI * 0.5), OpenRAVE::Vector()); // Z pointing to the left side of the directed segment (p0, p1)
+            _AppendFclHalfspaceCollsionObject(trans, contents);
+        }
+        _AppendFclHalfspaceCollsionObject(OpenRAVE::Transform(OpenRAVE::Vector(1, 0, 0, 0), OpenRAVE::Vector(0, 0, -info._vGeomData.y * 0.5)), contents);
+        _AppendFclHalfspaceCollsionObject(OpenRAVE::Transform(OpenRAVE::Vector(0, 1, 0, 0), OpenRAVE::Vector(0, 0, info._vGeomData.y * 0.5)), contents);
+        return std::make_shared<fcl::Container>(contents);
+    }
     case OpenRAVE::GT_ConicalFrustum:
     case OpenRAVE::GT_Axial:
     case OpenRAVE::GT_TriMesh:

plugins/oderave/odespace.h

@@ -642,6 +642,10 @@ class ODESpace : public boost::enable_shared_from_this<ODESpace>
         case OpenRAVE::GT_Cylinder:
             odegeom = dCreateCylinder(0,info._vGeomData.x,info._vGeomData.y);
             break;
+        case OpenRAVE::GT_Capsule:
+            odegeom = dCreateCapsule(0,info._vGeomData.x,info._vGeomData.y);
+            break;
+        case OpenRAVE::GT_Prism:
         case OpenRAVE::GT_ConicalFrustum:
         case OpenRAVE::GT_Axial:
         case OpenRAVE::GT_Container:

plugins/qtcoinrave/item.cpp

@@ -260,6 +260,8 @@ void KinBodyItem::Load()
                     psep->addChild(cy);
                     break;
                 }
+                case GT_Prism:
+                case GT_Capsule:
                 case GT_ConicalFrustum:
                 case GT_Axial:
                 case GT_Cage:

plugins/qtosgrave/osgrenderitem.cpp

@@ -442,7 +442,19 @@ void KinBodyItem::Load()
                     pgeometrydata->addChild(geode.get());
                     break;
                 }
+                case GT_Capsule: {
+                    // make SoCylinder point towards z, not y
+                    osg::Capsule* cy = new osg::Capsule();
+                    cy->setRadius(orgeom->GetCapsuleRadius());
+                    cy->setHeight(orgeom->GetCapsuleHeight());
+                    osg::ref_ptr<osg::Geode> geode = new osg::Geode;
+                    osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable(cy);
+                    geode->addDrawable(sd.get());
+                    pgeometrydata->addChild(geode.get());
+                    break;
+                }
                 //  Extract geometry from collision Mesh
+                case GT_Prism:
                 case GT_ConicalFrustum:
                 case GT_Axial:
                 case GT_Cage:

python/bindings/include/openravepy/openravepy_jointinfo.h

@@ -76,6 +76,9 @@ class PyGeometryInfo
     object GetConicalFrustumTopRadius() const;
     object GetConicalFrustumBottomRadius() const;
     object GetConicalFrustumHeight() const;
+    object GetPrismHeight() const;
+    object GetCapsuleRadius() const;
+    object GetCapsuleHeight() const;
     object GetCollisionMesh();
 
     std::string __repr__();
@@ -299,6 +302,9 @@ class PyGeometry
     dReal GetConicalFrustumTopRadius() const;
     dReal GetConicalFrustumBottomRadius() const;
     dReal GetConicalFrustumHeight() const;
+    dReal GetPrismHeight() const;
+    dReal GetCapsuleRadius() const;
+    dReal GetCapsuleHeight() const;
     object GetBoxExtents() const;
     object GetContainerOuterExtents() const;
     object GetContainerInnerExtents() const;

python/bindings/openravepy_kinbody.cpp

@@ -542,6 +542,21 @@ object PyGeometryInfo::GetConicalFrustumHeight() const {
     return _vGeomData[py::to_object(2)];
 }
 
+object PyGeometryInfo::GetPrismHeight() const
+{
+    return _vGeomData[py::to_object(1)];
+}
+
+object PyGeometryInfo::GetCapsuleRadius() const
+{
+    return _vGeomData[py::to_object(0)];
+}
+
+object PyGeometryInfo::GetCapsuleHeight() const
+{
+    return _vGeomData[py::to_object(1)];
+}
+
 object PyGeometryInfo::GetCollisionMesh()
 {
     KinBody::GeometryInfoPtr pgeominfo = GetGeometryInfo();
@@ -1504,6 +1519,15 @@ dReal PyGeometry::GetConicalFrustumBottomRadius() const {
 dReal PyGeometry::GetConicalFrustumHeight() const {
     return _pgeometry->GetConicalFrustumHeight();
 }
+dReal PyGeometry::GetPrismHeight() const {
+    return _pgeometry->GetPrismHeight();
+}
+dReal PyGeometry::GetCapsuleRadius() const {
+    return _pgeometry->GetCapsuleRadius();
+}
+dReal PyGeometry::GetCapsuleHeight() const {
+    return _pgeometry->GetCapsuleHeight();
+}
 object PyGeometry::GetBoxExtents() const {
     return toPyVector3(_pgeometry->GetBoxExtents());
 }
@@ -4965,6 +4989,8 @@ void init_openravepy_kinbody()
                           .value("CalibrationBoard",GT_CalibrationBoard)
                           .value("Axial",GT_Axial)
                           .value("ConicalFrustum",GT_ConicalFrustum)
+                          .value("Prism",GT_Prism)
+                          .value("Capsule",GT_Capsule)
     ;
 
 #ifdef USE_PYBIND11_PYTHON_BINDINGS
@@ -5137,6 +5163,9 @@ void init_openravepy_kinbody()
                           .def("GetConicalFrustumTopRadius",&PyGeometryInfo::GetConicalFrustumTopRadius, DOXY_FN(GeomeryInfo,GetConicalFrustumTopRadius))
                           .def("GetConicalFrustumBottomRadius",&PyGeometryInfo::GetConicalFrustumBottomRadius, DOXY_FN(GeomeryInfo,GetConicalFrustumBottomRadius))
                           .def("GetConicalFrustumHeight",&PyGeometryInfo::GetConicalFrustumHeight, DOXY_FN(GeomeryInfo,GetConicalFrustumHeight))
+                          .def("GetPrismHeight",&PyGeometryInfo::GetPrismHeight, DOXY_FN(GeomeryInfo,GetPrismHeight))
+                          .def("GetCapsuleRadius",&PyGeometryInfo::GetCapsuleRadius, DOXY_FN(GeomeryInfo,GetCapsuleRadius))
+                          .def("GetCapsuleHeight",&PyGeometryInfo::GetCapsuleHeight, DOXY_FN(GeomeryInfo,GetCapsuleHeight))
                           .def("GetCollisionMesh",&PyGeometryInfo::GetCollisionMesh, DOXY_FN(GeomeryInfo,GetCollisionMesh))
 #ifdef USE_PYBIND11_PYTHON_BINDINGS
                           .def("SerializeJSON", &PyGeometryInfo::SerializeJSON,
@@ -6293,6 +6322,9 @@ void init_openravepy_kinbody()
                                   .def("GetConicalFrustumTopRadius",&PyGeometry::GetConicalFrustumTopRadius, DOXY_FN(KinBody::Link::Geometry,GetConicalFrustumTopRadius))
                                   .def("GetConicalFrustumBottomRadius",&PyGeometry::GetConicalFrustumBottomRadius, DOXY_FN(KinBody::Link::Geometry,GetConicalFrustumBottomRadius))
                                   .def("GetConicalFrustumHeight",&PyGeometry::GetConicalFrustumHeight, DOXY_FN(KinBody::Link::Geometry,GetConicalFrustumHeight))
+                                  .def("GetPrismHeight",&PyGeometry::GetPrismHeight, DOXY_FN(KinBody::Link::Geometry,GetPrismHeight))
+                                  .def("GetCapsuleRadius",&PyGeometry::GetCapsuleRadius, DOXY_FN(KinBody::Link::Geometry,GetCapsuleRadius))
+                                  .def("GetCapsuleHeight",&PyGeometry::GetCapsuleHeight, DOXY_FN(KinBody::Link::Geometry,GetCapsuleHeight))
                                   .def("GetBoxExtents",&PyGeometry::GetBoxExtents, DOXY_FN(KinBody::Link::Geometry,GetBoxExtents))
                                   .def("GetContainerOuterExtents",&PyGeometry::GetContainerOuterExtents, DOXY_FN(KinBody::Link::Geometry,GetContainerOuterExtents))
                                   .def("GetContainerInnerExtents",&PyGeometry::GetContainerInnerExtents, DOXY_FN(KinBody::Link::Geometry,GetContainerInnerExtents))

src/libopenrave-core/colladaparser/colladawriter.cpp

@@ -2139,7 +2139,10 @@ class ColladaWriter : public daeErrorHandler
                 pcylinder->add("height")->setCharData(boost::lexical_cast<std::string>(geom->GetCylinderHeight()));
                 break;
             }
+            case GT_Prism:
+            case GT_Capsule:
             case GT_Axial:
+            case GT_ConicalFrustum:
             case GT_None:
             case GT_TriMesh:
                 // don't add anything

src/libopenrave/kinbody.cpp

@@ -425,7 +425,8 @@ void KinBody::KinBodyInfo::_DeserializeReadableInterface(const std::string& id,
         return;
     }
     if (rReadable.IsString()) {
-        StringReadablePtr pStringReadable(new StringReadable(id, rReadable.GetString()));
+        // When parsing c-strings as readables need to make sure we use the pointer + length constructor to handle strings with null bytes
+        StringReadablePtr pStringReadable(new StringReadable(id, rReadable.GetString(), rReadable.GetStringLength()));
         _mReadableInterfaces[id] = pStringReadable;
         return;
     }