WIP: Do not assume all-zero/initial robot pose as non-self-colliding pose and make non-self-colliding pose configurable

include/openrave/kinbody.h

@@ -2034,6 +2034,64 @@ class OPENRAVE_API KinBody : public InterfaceBase
     typedef boost::shared_ptr<KinBody::Joint const> JointConstPtr;
     typedef boost::weak_ptr<KinBody::Joint> JointWeakPtr;
 
+    /// \brief Holds a joint value set representing a KinBody/Robot pose
+    class OPENRAVE_API PositionConfiguration : public InfoBase
+    {
+public:
+        PositionConfiguration() = default;
+        virtual ~PositionConfiguration() = default;
+
+        void Reset() override;
+        void SerializeJSON(rapidjson::Value& value, rapidjson::Document::AllocatorType& allocator, dReal fUnitScale, int options=0) const override;
+        void DeserializeJSON(const rapidjson::Value& value, dReal fUnitScale, int options) override;
+
+        inline const std::string& GetId() const {
+            return _id;
+        }
+        inline const std::string& GetName() const {
+            return name;
+        }
+
+        bool operator==(const PositionConfiguration& other) const;
+        bool operator!=(const PositionConfiguration& other) const;
+
+        inline void Swap(PositionConfiguration& rhs) {
+            _id.swap(rhs._id);
+            name.swap(rhs.name);
+            jointConfigurationStates.swap(rhs.jointConfigurationStates);
+        }
+
+        class JointConfigurationState : public InfoBase
+        {
+public:
+            JointConfigurationState() = default;
+            virtual ~JointConfigurationState() = default;
+
+            void Reset() override;
+            void SerializeJSON(rapidjson::Value& value, rapidjson::Document::AllocatorType& allocator, dReal fUnitScale, int options=0) const override;
+            void DeserializeJSON(const rapidjson::Value& value, dReal fUnitScale, int options) override;
+
+            bool operator==(const JointConfigurationState& other) const;
+            bool operator!=(const JointConfigurationState& other) const;
+
+            std::string GetResolvedJointName() const;
+
+            std::string _id; ///< id of joint configuration state, for incremental update
+            std::string jointName; ///< name of the joint. If the joint belong to a connectedBody, then its resolved name is connectedBodyName+"_"+jointName
+            int jointAxis = 0;
+            dReal jointValue = 0.0;
+            std::string connectedBodyName; ///< Name of the connected body the joint comes from. Set to empty if the joint belongs to a robot, not a connected body.
+        };
+        typedef boost::shared_ptr<JointConfigurationState> JointConfigurationStatePtr;
+        typedef boost::shared_ptr<JointConfigurationState const> JointConfigurationStateConstPtr;
+
+        std::string _id; ///< unique id of the configuration used to identify it when changing it.
+        std::string name; ///< name of the configuration
+        std::vector<JointConfigurationState> jointConfigurationStates; ///< joint name to joint values mapping
+    };
+    typedef boost::shared_ptr<PositionConfiguration> PositionConfigurationPtr;
+    typedef boost::shared_ptr<PositionConfiguration const> PositionConfigurationConstPtr;
+
     /// \brief holds all user-set attached sensor information used to initialize the AttachedSensor class.
     ///
     /// This is serializable and independent of environment.
@@ -2205,6 +2263,8 @@ class OPENRAVE_API KinBody : public InterfaceBase
         std::vector<LinkInfoPtr> _vLinkInfos; ///< list of pointers to LinkInfo
         std::vector<JointInfoPtr> _vJointInfos; ///< list of pointers to JointInfo
 
+        std::vector<PositionConfigurationPtr> _vNonSelfCollidingPositionConfigurations; ///< list of non-self-colliding position configurations
+
         std::map<std::string, ReadablePtr> _mReadableInterfaces; ///< readable interface mapping
 
         bool _isRobot = false; ///< true if should create a RobotBasePtr
@@ -2436,6 +2496,9 @@ class OPENRAVE_API KinBody : public InterfaceBase
     /// \param dofindices the dof indices to return the values for. If empty, will compute for all the dofs
     void GetDOFVelocities(std::vector<dReal>& v, const std::vector<int>& dofindices = std::vector<int>()) const;
 
+    /// \brief Returns the current position configuration
+    virtual void GetPositionConfiguration(PositionConfiguration& positionConfiguration) const;
+
     /// \brief Returns all the joint limits as organized by the DOF indices.
     ///
     /// \param dofindices the dof indices to return the values for. If empty, will compute for all the dofs
@@ -3433,6 +3496,13 @@ class OPENRAVE_API KinBody : public InterfaceBase
 
     void _SetAdjacentLinksInternal(int linkindex0, int linkindex1);
 
+    void _CalculateAdjacentLinkFlagsFromNonSelfCollidingPositionConfigurations(std::vector<bool>& adjacentLinkFlags) const;
+
+    /// \brief Returns a full list of DOFs which values are determinable given an initial list of such DOFs
+    /// \param[in,out] isDOFValueDeterminableList List of flags which indicate whether DOF values are determinable. Takes an initial list as input, and returns a full list as output. Size must match GetDOF().
+    /// \return True if the list of DOFs has been converged, otherwise false.
+    bool _ResolveDOFValueDeterminableFlags(std::vector<bool>& isDOFValueDeterminableList, int maxNumIterations) const;
+
     std::string _name; ///< name of body
 
     std::vector<JointPtr> _vecjoints; ///< \see GetJoints
@@ -3462,7 +3532,8 @@ class OPENRAVE_API KinBody : public InterfaceBase
     mutable boost::array<std::vector<int>, 4> _vNonAdjacentLinks; ///< contains cached versions of the non-adjacent links depending on values in AdjacentOptions. Declared as mutable since data is cached.
     mutable boost::array<std::set<int>, 4> _cacheSetNonAdjacentLinks; ///< used for caching return value of GetNonAdjacentLinks.
     mutable int _nNonAdjacentLinkCache; ///< specifies what information is currently valid in the AdjacentOptions.  Declared as mutable since data is cached. If 0x80000000 (ie < 0), then everything needs to be recomputed including _setNonAdjacentLinks[0].
-    std::vector<Transform> _vInitialLinkTransformations; ///< the initial transformations of each link specifying at least one pose where the robot is collision free
+    typedef std::pair<PositionConfigurationPtr, std::vector<Transform> > PositionConfigurationAndLinkTransformations;
+    std::vector<PositionConfigurationAndLinkTransformations> _vNonSelfCollidingPositionConfigurationsAndLinkTransformations; ///< list of non-self-colliding position configurations and corresponding link transformations
 
     mutable std::vector<int8_t> _vAttachedVisitedCache; ///< cache
     mutable std::vector<std::pair<Vector,Vector> > _vVelocitiesCache;

include/openrave/robot.h

@@ -993,6 +993,9 @@ class OPENRAVE_API RobotBase : public KinBody
     /// \brief initializes a robot with info structure
     virtual bool InitFromRobotInfo(const RobotBaseInfo& info);
 
+    /// \brief Returns the current position configuration
+    virtual void GetPositionConfiguration(PositionConfiguration& positionConfiguration) const;
+
     /// \brief Returns the manipulators of the robot
     virtual const std::vector<ManipulatorPtr>& GetManipulators() const;
 

python/bindings/include/openravepy/openravepy_jointinfo.h

@@ -490,6 +490,54 @@ class PyJoint
     int __hash__();
 };
 
+class PyPositionConfiguration_JointConfigurationState
+{
+public:
+    PyPositionConfiguration_JointConfigurationState() = default;
+    PyPositionConfiguration_JointConfigurationState(const KinBody::PositionConfiguration::JointConfigurationState& jointConfigurationState);
+
+    KinBody::PositionConfiguration::JointConfigurationStatePtr GetJointConfigurationState() const;
+    object SerializeJSON(dReal fUnitScale = 1.0, object options = py::none_());
+    void DeserializeJSON(object obj, dReal fUnitScale = 1.0, object options = py::none_());
+
+    std::string __str__();
+    bool __eq__(OPENRAVE_SHARED_PTR<PyPositionConfiguration_JointConfigurationState> p);
+    bool __ne__(OPENRAVE_SHARED_PTR<PyPositionConfiguration_JointConfigurationState> p);
+
+    object _id = py::none_();
+    object jointName = py::none_();
+    int jointAxis = 0;
+    dReal jointValue = 0.0;
+    object connectedBodyName = py::none_();
+
+private:
+    void _Update(const KinBody::PositionConfiguration::JointConfigurationState& jointConfigurationState);
+};
+typedef OPENRAVE_SHARED_PTR<PyPositionConfiguration_JointConfigurationState> PyPositionConfiguration_JointConfigurationStatePtr;
+
+class PyPositionConfiguration
+{
+public:
+    PyPositionConfiguration() = default;
+    PyPositionConfiguration(const KinBody::PositionConfiguration& positionConfiguration);
+
+    KinBody::PositionConfigurationPtr GetPositionConfiguration() const;
+    object SerializeJSON(dReal fUnitScale = 1.0, object options = py::none_());
+    void DeserializeJSON(object obj, dReal fUnitScale = 1.0, object options = py::none_());
+
+    std::string __str__();
+    bool __eq__(OPENRAVE_SHARED_PTR<PyPositionConfiguration> p);
+    bool __ne__(OPENRAVE_SHARED_PTR<PyPositionConfiguration> p);
+
+    object _id = py::none_();
+    object name = py::none_();
+    py::list jointConfigurationStates;
+
+private:
+    void _Update(const KinBody::PositionConfiguration& positionConfiguration);
+};
+typedef OPENRAVE_SHARED_PTR<PyPositionConfiguration> PyPositionConfigurationPtr;
+
 class PyKinBodyStateSaver
 {
     PyEnvironmentBasePtr _pyenv;

python/bindings/include/openravepy/openravepy_kinbody.h

@@ -157,6 +157,7 @@ class PyKinBody : public PyInterfaceBase
         bool _isRobot = false;
         bool _isPartial = true;
         py::object _dofValues = py::none_();
+        py::object _vNonSelfCollidingPositionConfigurations = py::none_();
         py::object _readableInterfaces = py::none_();
         virtual std::string __str__();
         virtual py::object __unicode__();
@@ -201,6 +202,7 @@ class PyKinBody : public PyInterfaceBase
     py::object GetDOFValues(py::object oindices) const;
     py::object GetDOFVelocities() const;
     py::object GetDOFVelocities(py::object oindices) const;
+    py::object GetPositionConfiguration() const;
     py::object GetDOFLimits() const;
     py::object GetDOFVelocityLimits() const;
     py::object GetDOFAccelerationLimits() const;
@@ -326,6 +328,7 @@ class PyKinBody : public PyInterfaceBase
     py::object GetURI() const;
     py::object GetNonAdjacentLinks() const;
     py::object GetNonAdjacentLinks(int adjacentoptions) const;
+    bool AreAdjacentLinks(int linkindex0, int linkindex1) const;
     void SetAdjacentLinks(int linkindex0, int linkindex1);
     void SetAdjacentLinksCombinations(py::object olinkIndices);
     py::object GetAdjacentLinks() const;

python/bindings/include/openravepy/openravepy_robotbase.h

@@ -297,6 +297,8 @@ class PyRobotBase : public PyKinBody
 
     bool Init(object olinkinfos, object ojointinfos, object omanipinfos, object oattachedsensorinfos, const std::string& uri=std::string());
 
+    py::object GetPositionConfiguration() const;
+
     object GetManipulators();
 
     object GetManipulators(const std::string& manipname);