Merge branch '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 158)
-set (OPENRAVE_VERSION_PATCH 1)
+set (OPENRAVE_VERSION_MINOR 160)
+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}")
@@ -114,22 +114,6 @@ include(GNUInstallDirs)
 
 find_package(PkgConfig) # pkg_check_modules
 
-check_cxx_source_compiles(" 
-#include <mutex> 
-int main() 
-{ 
-    std::scoped_lock lock;
-    return 0;
-}"
-OPENRAVE_STD_SCOPED_LOCK
-)
-# need to convert to int to set config.h
-if(OPENRAVE_STD_SCOPED_LOCK)
-  set(OPENRAVE_STD_SCOPED_LOCK 1)
-else()
-  set(OPENRAVE_STD_SCOPED_LOCK 0)
-endif()
-
 check_cxx_source_compiles(" 
 #include <string_view> 
 int main() 
@@ -139,6 +123,7 @@ int main()
 }"
 OPENRAVE_STD_STRING_VIEW
 )
+# need to convert to int to set config.h
 if(OPENRAVE_STD_STRING_VIEW)
   set(OPENRAVE_STD_STRING_VIEW 1)
 else()

config.h.in

@@ -71,7 +71,6 @@
 #define OPENRAVE_CURL @OPENRAVE_CURL@
 
 #define OPENRAVE_ENVIRONMENT_RECURSIVE_LOCK @OPENRAVE_ENVIRONMENT_RECURSIVE_LOCK@
-#define OPENRAVE_STD_SCOPED_LOCK @OPENRAVE_STD_SCOPED_LOCK@
 #define OPENRAVE_STD_STRING_VIEW @OPENRAVE_STD_STRING_VIEW@
 
 #endif

docs/source/changelog.rst

@@ -3,6 +3,25 @@
 ChangeLog
 #########
 
+Version 0.160.0
+===============
+
+- Add new functions for AddKinBody/AddRobot to specify an exact environmentBodyIndex.
+- Use std::unique_lock instead of std::scoped_lock. Remove boost recursive mutex.
+
+Version 0.159.1
+===============
+
+- Fix the problem that connected body resolved joint names that are empty are not skipped in `_UpdateConnectedBodyInfo`.
+- Fix the problem that `CompareTransform` does not consider the quaternions `quat` and `-quat` to be the same rotation.
+- Fix wrong ComputeInverseDynamics for Prismatic joint.
+
+Version 0.159.0
+===============
+
+- Add HasReadableInterface API to minimize copying when using python bindings
+- Make the connected body's joint properties (such as velocity/acceleration limits) persistent by always keeping the connected body's `_info` up to date.
+
 Version 0.158.1
 ===============
 

include/openrave/environment.h

@@ -23,23 +23,16 @@
 #define  OPENRAVE_ENVIRONMENTBASE_H
 
 #include <openrave/config.h>
+#include <mutex>
 
 namespace OpenRAVE {
 
 #if OPENRAVE_ENVIRONMENT_RECURSIVE_LOCK
-#if OPENRAVE_STD_SCOPED_LOCK
-#include <mutex>
 using EnvironmentMutex = ::std::recursive_mutex;
 using EnvironmentLock  = ::std::unique_lock<std::recursive_mutex>;
 using defer_lock_t     = ::std::defer_lock_t;
 using try_to_lock_t    = ::std::try_to_lock_t;
 #else
-using EnvironmentMutex = ::boost::recursive_try_mutex;
-using EnvironmentLock  = EnvironmentMutex::scoped_lock;
-using defer_lock_t     = ::boost::defer_lock_t;
-using try_to_lock_t    = ::boost::try_to_lock_t;
-#endif // OPENRAVE_STD_SCOPED_LOCK
-#else
 using EnvironmentMutex = ::std::mutex;
 using EnvironmentLock  = ::std::unique_lock<std::mutex>;
 using defer_lock_t     = ::std::defer_lock_t;
@@ -482,6 +475,22 @@ class OPENRAVE_API EnvironmentBase : public boost::enable_shared_from_this<Envir
 
     virtual void Add(InterfaceBasePtr pinterface, bool bAnonymous, const std::string& cmdargs=std::string()) RAVE_DEPRECATED;
 
+    /** \brief Add an body to the environment
+        \param[in] pbody the pointer to an initialized body
+        \param[in] addMode One of IAM_X
+        \param[in] requestedEnvironmentBodyIndex if positive and none of existing body uses it, this is assigned to pbody. If positive and existing body uses it, exception is thrown. If non-positive, environment body index is decided internally.
+        \throw openrave_exception Throw if interface is invalid or already added
+     */
+    virtual void AddKinBody(KinBodyPtr pbody, InterfaceAddMode addMode, int requestedEnvironmentBodyIndex) = 0;
+
+    /** \brief Add an robot to the environment
+        \param[in] probot the pointer to an initialized robot
+        \param[in] addMode One of IAM_X
+        \param[in] requestedEnvironmentBodyIndex if positive and none of existing body uses it, this is assigned to probot. If positive and existing body uses it, exception is thrown. If non-positive, environment body index is decided internally.
+        \throw openrave_exception Throw if interface is invalid or already added
+     */
+    virtual void AddRobot(RobotBasePtr probot, InterfaceAddMode addMode, int requestedEnvironmentBodyIndex) = 0;
+
     /// \brief bodycallback(body, action)
     ///
     /// \param body KinBodyPtr
@@ -762,7 +771,7 @@ class OPENRAVE_API EnvironmentBase : public boost::enable_shared_from_this<Envir
     ///
     /// extents are half the width, height, and depth of the box
     /// \return handle to plotted boxes, graph is removed when handle is destroyed (goes out of scope). This requires the user to always store the handle in a persistent variable if the plotted graphics are to remain on the viewer.
-    virtual OpenRAVE::GraphHandlePtr drawboxarray(const std::vector<RaveVector<float>>& vpos, const RaveVector<float>& vextents) = 0;
+    virtual OpenRAVE::GraphHandlePtr drawboxarray(const std::vector<RaveVector<float>>& vpos, const RaveVector<float>& vextents, const std::vector<RaveVector<float>>& colors = {}) = 0;
 
     /// \brief Draws a AABB. <b>[multi-thread safe]</b>
     ///

include/openrave/geometry.h

@@ -493,8 +493,11 @@ class RaveTransform
 
     /// \brief return true if any element of transform is different by mor ethan epsilon, otherwise false.
     inline bool CompareTransform(const RaveTransform<T>& rhs, T epsilon) const {
-        return RaveFabs(trans.x - rhs.trans.x) > epsilon || RaveFabs(trans.y - rhs.trans.y) > epsilon || RaveFabs(trans.z - rhs.trans.z) > epsilon ||
-               RaveFabs(rot.x - rhs.rot.x) > epsilon || RaveFabs(rot.y - rhs.rot.y) > epsilon || RaveFabs(rot.z - rhs.rot.z) > epsilon || RaveFabs(rot.w - rhs.rot.w) > epsilon;
+        return (RaveFabs(trans.x - rhs.trans.x) > epsilon
+		|| RaveFabs(trans.y - rhs.trans.y) > epsilon
+		|| RaveFabs(trans.z - rhs.trans.z) > epsilon
+		|| ((RaveFabs(rot.x - rhs.rot.x) > epsilon || RaveFabs(rot.y - rhs.rot.y) > epsilon || RaveFabs(rot.z - rhs.rot.z) > epsilon || RaveFabs(rot.w - rhs.rot.w) > epsilon)
+		    && ((RaveFabs(rot.x + rhs.rot.x) > epsilon || RaveFabs(rot.y + rhs.rot.y) > epsilon || RaveFabs(rot.z + rhs.rot.z) > epsilon || RaveFabs(rot.w + rhs.rot.w) > epsilon))));
     }
 
     inline RaveTransform<T> inverse() const {

include/openrave/interface.h

@@ -91,6 +91,9 @@ class OPENRAVE_API ReadablesContainer
     /// \brief Returns the readable interface. <b>[multi-thread safe]</b>
     virtual ReadablePtr GetReadableInterface(const std::string& id) const;
 
+    /// \brief Returns whether a readable interface exists. <b>[multi-thread safe]</b>
+    virtual bool HasReadableInterface(const std::string& id) const;
+
     /// \brief Set a new readable interface and return the previously set interface if it exists. <b>[multi-thread safe]</b>
     virtual ReadablePtr SetReadableInterface(const std::string& id, ReadablePtr readable);
 

include/openrave/openraveexception.h

@@ -50,6 +50,7 @@ enum OpenRAVEErrorCode
     ORE_TimeDurationUnitInvalid = 18, ///< Cannot find the specific TimeDurationUnit
     ORE_AngleUnitInvalid = 19, ///< Cannot find the specific AngleUnit
     ORE_TimeStampUnitInvalid = 20, ///< Cannot find the specific TimeStampUnit
+    ORE_EnvironmentBodyIndexConflict=21, ///< body with same environment body index is trying to be added to the environment
 
     ORE_EnvironmentFormatUnrecognized = 0x0100, ///< the environment format to load is not recognized.
 

include/openrave/robot.h

@@ -862,7 +862,11 @@ class OPENRAVE_API RobotBase : public KinBody
         bool CanProvideManipulator(const std::string& resolvedManipulatorName) const;
 
 private:
+        /// \brief A callback function to update _info when the robot's properties change.
+        void _UpdateConnectedBodyInfo();
+
         ConnectedBodyInfo _info; ///< user specified data (to be serialized and saved), should not contain dynamically generated parameters.
+        UserDataPtr _updateInfoCallback; ///< callback registered to the robot to update _info whenever the robot properties (such as joint velocity/acceleration limits) change.
 
         std::string _nameprefix; ///< the name prefix to use for all the resolved link names. Initialized regardless of the active state of the connected body.
         std::string _dummyPassiveJointName; ///< the joint that is used to attach the connected body to the robot link

include/openrave/viewer.h

@@ -214,7 +214,7 @@ class OPENRAVE_API ViewerBase : public InterfaceBase
     virtual GraphHandlePtr drawlabel(const std::string& label, const RaveVector<float>& worldPosition, const RaveVector<float>& color = RaveVector<float>(0,0,0,1), float height = 0.05) OPENRAVE_DUMMY_IMPLEMENTATION;
 
     virtual GraphHandlePtr drawbox(const RaveVector<float>& vpos, const RaveVector<float>& vextents) OPENRAVE_DUMMY_IMPLEMENTATION;
-    virtual GraphHandlePtr drawboxarray(const std::vector<RaveVector<float>>& vpos, const RaveVector<float>& vextents) OPENRAVE_DUMMY_IMPLEMENTATION;
+    virtual GraphHandlePtr drawboxarray(const std::vector<RaveVector<float>>& vpos, const RaveVector<float>& vextents, const std::vector<RaveVector<float>>& vcolors) OPENRAVE_DUMMY_IMPLEMENTATION;
     virtual GraphHandlePtr drawaabb(const AABB& aabb, const RaveTransform<float>& transform, const RaveVector<float>& vcolor, float transparency) OPENRAVE_DUMMY_IMPLEMENTATION;
     virtual GraphHandlePtr drawobb(const OrientedBox& obb, const RaveVector<float>& vcolor, float transparency) OPENRAVE_DUMMY_IMPLEMENTATION;
     virtual GraphHandlePtr drawplane(const RaveTransform<float>& tplane, const RaveVector<float>& vextents, const boost::multi_array<float,3>& vtexture) OPENRAVE_DUMMY_IMPLEMENTATION;

plugins/qtcoinrave/qtcoinviewer.cpp

@@ -1103,7 +1103,7 @@ GraphHandlePtr QtCoinViewer::drawbox(const RaveVector<float>& vpos, const RaveVe
     pmsg->callerexecute(false);
     return GraphHandlePtr(new PrivateGraphHandle(shared_viewer(), handle));
 }
-GraphHandlePtr QtCoinViewer::drawboxarray(const std::vector<RaveVector<float> >& vpos, const RaveVector<float>& vextents) {
+GraphHandlePtr QtCoinViewer::drawboxarray(const std::vector<RaveVector<float> >& vpos, const RaveVector<float>& vextents, const std::vector<RaveVector<float>>& vcolors) {
     // not implemented
     return GraphHandlePtr();
 }

plugins/qtcoinrave/qtcoinviewer.h

@@ -170,7 +170,7 @@ class QtCoinViewer : public QMainWindow, public ViewerBase
 
     virtual GraphHandlePtr drawarrow(const RaveVector<float>& p1, const RaveVector<float>& p2, float fwidth, const RaveVector<float>& color);
     virtual GraphHandlePtr drawbox(const RaveVector<float>& vpos, const RaveVector<float>& vextents);
-    virtual GraphHandlePtr drawboxarray(const std::vector<RaveVector<float>>& vpos, const RaveVector<float>& vextents);
+    virtual GraphHandlePtr drawboxarray(const std::vector<RaveVector<float>>& vpos, const RaveVector<float>& vextents, const std::vector<RaveVector<float>>& vcolors);
     virtual GraphHandlePtr drawplane(const RaveTransform<float>& tplane, const RaveVector<float>& vextents, const boost::multi_array<float,3>& vtexture);
     virtual GraphHandlePtr drawtrimesh(const float* ppoints, int stride, const int* pIndices, int numTriangles, const RaveVector<float>& color);
     virtual GraphHandlePtr drawtrimesh(const float* ppoints, int stride, const int* pIndices, int numTriangles, const boost::multi_array<float,2>& colors);

plugins/qtosgrave/qtosgviewer.cpp

@@ -1771,19 +1771,24 @@ GraphHandlePtr QtOSGViewer::drawbox(const RaveVector<float>& vpos, const RaveVec
     return GraphHandlePtr(new PrivateGraphHandle(shared_viewer(), handle));
 }
 
-void QtOSGViewer::_DrawBoxArray(OSGSwitchPtr handle, const std::vector<RaveVector<float> >& vpos, const RaveVector<float>& vextents, bool bUsingTransparency)
+void QtOSGViewer::_DrawBoxArray(OSGSwitchPtr handle, const std::vector<RaveVector<float> >& vpos, const RaveVector<float>& vextents, const std::vector<RaveVector<float> >& vcolors, bool bUsingTransparency)
 {
     OSGMatrixTransformPtr trans(new osg::MatrixTransform());
     osg::ref_ptr<osg::Geode> geode(new osg::Geode());
-
+    bool size_mismatch = vcolors.size() != vpos.size();
+    const RaveVector<float> vdefaultcolor(1,0.5,0.5,1);
     for (size_t i = 0; i < vpos.size(); i++) {
         const RaveVector<float>& pos = vpos[i];
         osg::ref_ptr<osg::Box> box = new osg::Box();
         box->setHalfLengths(osg::Vec3(vextents.x, vextents.y, vextents.z));
         box->setCenter(osg::Vec3(pos.x, pos.y, pos.z));
-
+        
         osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable(box.get());
-        sd->setColor(osg::Vec4f(0.33203125f, 0.5f, 0.898437f, 1.0f));
+        if (size_mismatch) {
+            sd->setColor(osg::Vec4f(vdefaultcolor.x, vdefaultcolor.y, vdefaultcolor.z, vdefaultcolor.w));
+        } else {
+            sd->setColor(osg::Vec4f(vcolors[i].x, vcolors[i].y, vcolors[i].z, vcolors[i].w));
+        }
         geode->addDrawable(sd);
     }
 
@@ -1796,10 +1801,10 @@ void QtOSGViewer::_DrawBoxArray(OSGSwitchPtr handle, const std::vector<RaveVecto
     _posgWidget->GetFigureRoot()->insertChild(0, handle);
 }
 
-GraphHandlePtr QtOSGViewer::drawboxarray(const std::vector<RaveVector<float> >& vpos, const RaveVector<float>& vextents)
+GraphHandlePtr QtOSGViewer::drawboxarray(const std::vector<RaveVector<float> >& vpos, const RaveVector<float>& vextents, const std::vector<RaveVector<float> >& vcolors)
 {
     OSGSwitchPtr handle = _CreateGraphHandle();
-    _PostToGUIThread(boost::bind(&QtOSGViewer::_DrawBoxArray, this, handle, vpos, vextents, false), ViewerCommandPriority::MEDIUM); // copies ref counts
+    _PostToGUIThread(boost::bind(&QtOSGViewer::_DrawBoxArray, this, handle, vpos, vextents, vcolors, false), ViewerCommandPriority::MEDIUM); // copies ref counts
     return GraphHandlePtr(new PrivateGraphHandle(shared_viewer(), handle));
 }
 

plugins/qtosgrave/qtosgviewer.h

@@ -80,7 +80,7 @@ class QtOSGViewer : public QMainWindow, public ViewerBase
 
     virtual GraphHandlePtr drawplane(const RaveTransform<float>& tplane, const RaveVector<float>& vextents, const boost::multi_array<float,3>& vtexture);
     virtual GraphHandlePtr drawbox(const RaveVector<float>& vpos, const RaveVector<float>& vextents);
-    virtual GraphHandlePtr drawboxarray(const std::vector<RaveVector<float> >& vpos, const RaveVector<float>& vextents);
+    virtual GraphHandlePtr drawboxarray(const std::vector<RaveVector<float> >& vpos, const RaveVector<float>& vextents, const std::vector<RaveVector<float> >& vcolors);
     virtual GraphHandlePtr drawaabb(const AABB& aabb, const RaveTransform<float>& transform, const RaveVector<float>& vcolor, float transparency);
     virtual GraphHandlePtr drawobb(const OrientedBox& obb, const RaveVector<float>& vcolor, float transparency);
 
@@ -338,7 +338,7 @@ public slots:
     virtual void _SetTriangleMesh(const float* ppoints, int stride, const int* pIndices, int numTriangles, osg::ref_ptr<osg::Vec3Array> osgvertices, osg::ref_ptr<osg::DrawElementsUInt> osgindices);
     virtual void _DrawLabel(OSGSwitchPtr handle, const std::string& label, const RaveVector<float>& worldPosition, const RaveVector<float>& color = RaveVector<float>(0,0,0,1), float height=0.05);
     virtual void _DrawBox(OSGSwitchPtr handle, const RaveVector<float>& vextents, const RaveTransform<float>& pose, const RaveVector<float>& color, float transparency);
-    virtual void _DrawBoxArray(OSGSwitchPtr handle, const std::vector<RaveVector<float> >& vpos, const RaveVector<float>& vextents, bool bUsingTransparency);
+    virtual void _DrawBoxArray(OSGSwitchPtr handle, const std::vector<RaveVector<float> >& vpos, const RaveVector<float>& vextents, const std::vector<RaveVector<float> >& vcolors, bool bUsingTransparency);
     virtual void _DrawPlane(OSGSwitchPtr handle, const RaveTransform<float>& tplane, const RaveVector<float>& vextents, const boost::multi_array<float,3>& vtexture);
     virtual void _DrawArrow(OSGSwitchPtr handle, const RaveVector<float>& p1, const RaveVector<float>& p2, float fwidth, const RaveVector<float>& color);
 

python/bindings/include/openravepy/openravepy_environmentbase.h

@@ -217,8 +217,10 @@ class OPENRAVEPY_API PyEnvironmentBase : public OPENRAVE_ENABLE_SHARED_FROM_THIS
 
     void AddKinBody(PyKinBodyPtr pbody);
     void AddKinBody(PyKinBodyPtr pbody, bool bAnonymous);
+    void AddKinBody(PyKinBodyPtr pbody, py::object oAddMode, int requestedEnvironmentBodyIndex);
     void AddRobot(PyRobotBasePtr robot);
     void AddRobot(PyRobotBasePtr robot, bool bAnonymous);
+    void AddRobot(PyRobotBasePtr robot, py::object oAddMode, int requestedEnvironmentBodyIndex);
     void AddSensor(PySensorBasePtr sensor);
     void AddSensor(PySensorBasePtr sensor, bool bAnonymous);
     void AddViewer(PyViewerBasePtr viewer);
@@ -292,7 +294,7 @@ class OPENRAVEPY_API PyEnvironmentBase : public OPENRAVE_ENABLE_SHARED_FROM_THIS
     static size_t _getGraphColors(object ocolors, std::vector<float>&vcolors);
 
     /// returns the number of vectors
-    static size_t _getListVector(object odata, std::vector<RaveVector<float> >& vvectors);
+    static size_t _getListVector(object odata, std::vector<RaveVector<float> >& vvectors, size_t numcol);
 
     static std::pair<size_t,size_t> _getGraphPointsColors(object opoints, object ocolors, std::vector<float>&vpoints, std::vector<float>&vcolors);
 
@@ -307,7 +309,7 @@ class OPENRAVEPY_API PyEnvironmentBase : public OPENRAVE_ENABLE_SHARED_FROM_THIS
     object drawlabel(const std::string &label, object worldPosition, object ocolor=py::none_(), float height=0.05);
 
     object drawbox(object opos, object oextents, object ocolor=py::none_());
-    object drawboxarray(object opos, object oextents, object ocolor=py::none_());
+    object drawboxarray(object opos, object oextents, object ocolors=py::none_());
     object drawaabb(object oaabb, object otransform, object ocolor=py::none_(), float transparency=0.0f);
     object drawobb(object oobb, object ocolor=py::none_(), float transparency=0.0f);
 

python/bindings/include/openravepy/openravepy_int.h

@@ -641,6 +641,7 @@ class OPENRAVEPY_API PyReadablesContainer
 
     virtual py::object GetReadableInterfaces();
     virtual py::object GetReadableInterface(const std::string& xmltag);
+    virtual bool HasReadableInterface(const std::string& xmltag);
 
     virtual void SetReadableInterface(const std::string& xmltag, py::object oreadable);
 };