Draft: gather ik failure information in IkReturn

include/openrave/collisionchecker.h

@@ -66,6 +66,8 @@ class OPENRAVE_API CollisionReport
         Vector pos;  ///< where the contact occured
         Vector norm; ///< the normals of the faces
         dReal depth; ///< the penetration depth, positive means the surfaces are penetrating, negative means the surfaces are not colliding (used for distance queries)
+
+        void SaveToJson(rapidjson::Value& rContact, rapidjson::Document::AllocatorType& alloc) const;
     };
 
     CollisionReport() {
@@ -79,9 +81,15 @@ class OPENRAVE_API CollisionReport
     virtual void Reset(int coloptions = 0);
     virtual std::string __str__() const;
 
+    virtual void FillBodyNames();
+
+    void SaveToJson(rapidjson::Value& rCollisionReport, rapidjson::Document::AllocatorType& alloc) const;
+
     KinBody::LinkConstPtr plink1, plink2; ///< the colliding links if a collision involves a bodies. Collisions do not always occur with 2 bodies like ray collisions, so these fields can be empty.
 
     std::vector<std::pair<KinBody::LinkConstPtr, KinBody::LinkConstPtr> > vLinkColliding; ///< all link collision pairs. Set when CO_AllCollisions is enabled.
+    std::vector<std::pair<KinBody::GeometryConstPtr, KinBody::GeometryConstPtr> > vGeomColliding; ///< all colliding geometry pairs. Each pair corresponds to a colliding link pair in vLinkColliding so vGeomColliding.size() == vLinkColliding.size() should always evaluate to true. Set when CO_AllCollisions is enabled.
+    std::vector<std::pair<std::string, std::string> > vBodyColliding; ///< vBodyColliding[index] is a pair of kinbody names to which the links in vLinkColliding[index] belong. Not automatically generated. Can be initialized by CollisionReport::FillBodyNames.
 
     KinBody::GeometryConstPtr pgeom1, pgeom2; ///< the specified geometries hit for the given links
 
@@ -94,6 +102,7 @@ class OPENRAVE_API CollisionReport
 
     uint8_t nKeepPrevious; ///< if 1, will keep all previous data when resetting the collision checker. otherwise will reset
 
+    std::string bodyName1, bodyName2; /// the names of the bodies plink1 and plink2 belong to. Directly store the names here since if the parent has been destroyed, the parent's name cannot be obtained from plink.
 };
 
 typedef CollisionReport COLLISIONREPORT RAVE_DEPRECATED;

include/openrave/iksolver.h

@@ -36,7 +36,6 @@ enum IkFilterOptions
     IKFO_IgnoreEndEffectorCollisions=0x10, ///< \see IKFO_IgnoreEndEffectorEnvCollisions
     IKFO_IgnoreEndEffectorEnvCollisions=0x10, ///< will not check collision with the environment and the end effector links and bodies attached to the end effector links. The end effector links are defined by \ref RobotBase::Manipulator::GetChildLinks. Use this option when \ref RobotBase::Manipulator::CheckEndEffectorCollision has already been called, or it is ok for the end effector to collide given the IK constraints. Self-collisions between the moving links and end effector are still checked.
     IKFO_IgnoreEndEffectorSelfCollisions=0x20, ///< will not check self-collisions with the end effector. The end effector links are defined by \ref RobotBase::Manipulator::GetChildLinks. Use this option if it is ok for the end effector to collide given the IK constraints. Collisions between the moving links and end effector are still checked.
-    //IKFO_FillCollisionReports=0x1000, ///< if set, will fill the collision reports of the IkReturn structure (TODO)
 };
 
 /// \brief Return value for the ik filter that can be optionally set on an ik solver.
@@ -70,6 +69,100 @@ static const IkReturnAction IKFR_Reject RAVE_DEPRECATED = IKRA_Reject;
 static const IkReturnAction IKFR_Quit RAVE_DEPRECATED = IKRA_Quit;
 typedef IkReturnAction IkFilterReturn RAVE_DEPRECATED;
 
+class OPENRAVE_API IkFailureInfo
+{
+public:
+    IkFailureInfo()
+    {
+    }
+
+    inline void Init(const IkFailureInfo& ikFailureInfo)
+    {
+        _action = ikFailureInfo._action;
+        _vconfig = ikFailureInfo._vconfig;
+        _description = ikFailureInfo._description;
+        _mapdata = ikFailureInfo._mapdata;
+        InitCollisionReport(ikFailureInfo._preport);
+        if( ikFailureInfo.HasValidIkParam() ) {
+            SetIkParam(ikFailureInfo.GetIkParam());
+        }
+        else {
+            _bIkParamValid = false;
+        }
+    }
+
+    /// \brief clears the data. _action is left unchanged.
+    void Clear();
+
+    /// \brief initializes _preport according to the passed in report.
+    void InitCollisionReport(const CollisionReportPtr& preport);
+
+    void SetDescription(const std::string& description);
+
+    void SaveToJson(rapidjson::Value& rIkFailureInfo, rapidjson::Document::AllocatorType& alloc) const;
+
+    inline void SetIkParam(const IkParameterization& ikparam) {
+        if( ikparam.GetType() != IKP_None ) {
+            _ikparam = ikparam;
+            _bIkParamValid = true;
+        }
+        else {
+            _bIkParamValid = false;
+        }
+    }
+    inline bool HasValidIkParam() const {
+        return _bIkParamValid;
+    }
+    inline const IkParameterization& GetIkParam() const {
+        return _ikparam;
+    }
+
+    int GetIndex() const {
+        return _index;
+    }
+
+    typedef std::map<std::string, std::vector<dReal> > CustomData;
+    IkReturnAction _action = IKRA_Reject; ///< the IkReturnAction corresponding to this failure
+    std::vector< dReal > _vconfig; ///< the robot configuration that does not pass the checks.
+    CollisionReportPtr _preport;   ///< the collision report from when some collision checking fails.
+    std::string _description;      ///< a string describing the failure
+    CustomData _mapdata;           ///< stored additional information that does not fit elsewhere
+    int _index; // for debugging
+
+private:
+    IkParameterization _ikparam;   ///< the ikparam that fails (could be different from the ikparam given to FindIKSolutions call).
+    bool _bIkParamValid=false;     ///< a flag determining whether _ikparam is valid.
+};
+
+class OPENRAVE_API IkFailureAccumulator
+{
+public:
+    IkFailureAccumulator();
+
+    inline const size_t GetCurrentSize() const
+    {
+        return _nextIndex;
+    }
+
+    inline void ResetIndex(const size_t nextIndex=0)
+    {
+        _nextIndex = nextIndex;
+    }
+
+    /// \brief Retrieve ikFailureInfo from the specified index. Assume the input index is valid.
+    inline const IkFailureInfo& GetIkFailureInfo(size_t index) const
+    {
+        return *_vIkFailureInfos[index];
+    }
+
+    /// \brief Get the next available IkFailureInfo to fill in failure information.
+    IkFailureInfoPtr GetNextAvailableIkFailureInfoPtr();
+
+private:
+    std::vector<IkFailureInfoPtr> _vIkFailureInfos;
+    size_t _nextIndex = 0;
+};
+
 class OPENRAVE_API IkReturn
 {
 public:
@@ -86,12 +179,10 @@ class OPENRAVE_API IkReturn
     /// \brief appends the data of one IkReturn to this structure
     ///
     /// _action is untouched, _vsolution is overridden if non-empty
-    /// \return If data clashes, will output text and return false
+    /// \return true if data clashes. Also outputs text in such cases.
     bool Append(const IkReturn& r);
 
     /// \brief clears the data, leaves the _action unchanged
-    ///
-    /// if _preport is set, will call Reset on it.
     void Clear();
 
     typedef std::map<std::string, std::vector<dReal> > CustomData;
@@ -100,6 +191,7 @@ class OPENRAVE_API IkReturn
     CustomData _mapdata; ///< name/value pairs for custom data computed in the filters. Cascading filters using the same name will overwrite this until the last executed filter (with lowest priority).
     UserDataPtr _userdata; ///< if the name/value pairs are not enough, can further use a pointer to custom data. Cascading filters with valid _userdata pointers will overwrite this until the last executed filter (with lowest priority).
     //std::vector<CollisionReport> _reports; ///< all the reports that are written with the collision information if ik failed due to collisions. Only valid if _action has IKRA_RejectSelfCollision or IKRA_RejectEnvCollision set. (TODO)
+    std::vector<IkFailureInfoPtr> _vIkFailureInfos;
 };
 
 /** \brief <b>[interface]</b> Base class for all Inverse Kinematic solvers. <b>If not specified, method is not multi-thread safe.</b> See \ref arch_iksolver.
@@ -207,6 +299,7 @@ class OPENRAVE_API IkSolverBase : public InterfaceBase
         \return true if solution is found
      */
     virtual bool Solve(const IkParameterization& param, const std::vector<dReal>& q0, int filteroptions, IkReturnPtr ikreturn);
+    virtual bool Solve(const IkParameterization& param, const std::vector<dReal>& q0, int filteroptions, IkFailureAccumulatorPtr paccumulator, IkReturnPtr ikreturn);
 
     /** \brief Return all joint configurations for the given end effector transform.
 
@@ -230,6 +323,7 @@ class OPENRAVE_API IkSolverBase : public InterfaceBase
         \return true if at least one solution is found
      */
     virtual bool SolveAll(const IkParameterization& param, int filteroptions, std::vector<IkReturnPtr>& ikreturns);
+    virtual bool SolveAll(const IkParameterization& param, int filteroptions, IkFailureAccumulatorPtr paccumulator, std::vector<IkReturnPtr>& ikreturns);
 
     /** Return a joint configuration for the given end effector transform.
 
@@ -254,6 +348,7 @@ class OPENRAVE_API IkSolverBase : public InterfaceBase
         \return true if solution is found
      */
     virtual bool Solve(const IkParameterization& param, const std::vector<dReal>& q0, const std::vector<dReal>& vFreeParameters, int filteroptions, IkReturnPtr ikreturn);
+    virtual bool Solve(const IkParameterization& param, const std::vector<dReal>& q0, const std::vector<dReal>& vFreeParameters, int filteroptions, IkFailureAccumulatorPtr paccumulator, IkReturnPtr ikreturn);
 
     /** \brief Return all joint configurations for the given end effector transform.
 
@@ -281,6 +376,7 @@ class OPENRAVE_API IkSolverBase : public InterfaceBase
         \return true at least one solution is found
      */
     virtual bool SolveAll(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, int filteroptions, std::vector<IkReturnPtr>& ikreturns);
+    virtual bool SolveAll(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, int filteroptions, IkFailureAccumulatorPtr paccumulator, std::vector<IkReturnPtr>& ikreturns);
 
     /// \brief returns true if the solver supports a particular ik parameterization as input.
     virtual bool Supports(IkParameterizationType iktype) const OPENRAVE_DUMMY_IMPLEMENTATION;
@@ -301,7 +397,7 @@ class OPENRAVE_API IkSolverBase : public InterfaceBase
 
     /// \brief returns the kinematics structure hash this ik solver is encoded to. Checked with \ref RobotBase::Manipulator::GetKinematicsStructureHash()
     virtual const std::string& GetKinematicsStructureHash() const OPENRAVE_DUMMY_IMPLEMENTATION;
-    
+
 protected:
     inline IkSolverBasePtr shared_iksolver() {
         return boost::static_pointer_cast<IkSolverBase>(shared_from_this());

include/openrave/openrave.h

@@ -243,6 +243,8 @@ class SpaceSamplerBase;
 class IkParameterization;
 class ConfigurationSpecification;
 class IkReturn;
+class IkFailureInfo;
+class IkFailureAccumulator;
 class Readable;
 
 typedef boost::shared_ptr<CollisionReport> CollisionReportPtr;
@@ -298,6 +300,8 @@ typedef boost::weak_ptr<Readable> ReadableWeakPtr;
 typedef boost::shared_ptr<IkReturn> IkReturnPtr;
 typedef boost::shared_ptr<IkReturn const> IkReturnConstPtr;
 typedef boost::weak_ptr<IkReturn> IkReturnWeakPtr;
+typedef boost::shared_ptr<IkFailureInfo> IkFailureInfoPtr;
+typedef boost::shared_ptr<IkFailureAccumulator> IkFailureAccumulatorPtr;
 
 class BaseXMLReader;
 typedef boost::shared_ptr<BaseXMLReader> BaseXMLReaderPtr;
@@ -2386,6 +2390,8 @@ class OPENRAVE_API TriMesh
     AABB ComputeAABB() const;
     void serialize(std::ostream& o, int options=0) const;
 
+    void SerializeJSON(rapidjson::Value& rTriMesh, rapidjson::Document::AllocatorType& allocator, dReal fUnitScale, int options=0) const;
+
     friend OPENRAVE_API std::ostream& operator<<(std::ostream& O, const TriMesh &trimesh);
     friend OPENRAVE_API std::istream& operator>>(std::istream& I, TriMesh& trimesh);
 

include/openrave/planner.h

@@ -477,7 +477,7 @@ class OPENRAVE_API PlannerParameters : public BaseXMLReader, public Readable
     {
         return false;
     }
-    
+
     /// \brief output the planner parameters in a string (in XML format)
     ///
     /// \param options if 1 will skip writing the extra parameters
@@ -523,6 +523,8 @@ class OPENRAVE_API PlannerStatus
     PlannerStatus(const std::string& description, const uint32_t statusCode, CollisionReportPtr& report);
     PlannerStatus(const std::string& description, const uint32_t statusCode, const IkParameterization& ikparam);
     PlannerStatus(const std::string& description, const uint32_t statusCode, const IkParameterization& ikparam, CollisionReportPtr& report);
+    PlannerStatus(const std::string& description, const uint32_t statusCode, const IkParameterization& ikparam, const std::vector<IkFailureInfoPtr>& vIkFailureInfos);
+    PlannerStatus(const std::string& description, const uint32_t statusCode, const std::vector<IkFailureInfoPtr>& vIkFailureInfos);
     PlannerStatus(const std::string& description, const uint32_t statusCode, const std::vector<dReal>& jointValues);
     PlannerStatus(const std::string& description, const uint32_t statusCode, const std::vector<dReal>& jointValues, CollisionReportPtr& report);
 
@@ -551,6 +553,7 @@ class OPENRAVE_API PlannerStatus
     std::vector<dReal> jointValues;         // Optional, the robot's joint values in rad or m
     CollisionReportPtr report;              ///< Optional,  collision report at the time of the error. Ideally should contents contacts information.
     std::string errorOrigin;                // Auto, a string representing the code path of the error.
+    std::vector<IkFailureInfoPtr> vIkFailureInfos; ///< Optional, ikFailureInfos collected from the run.
 
     std::map< std::pair<KinBody::LinkConstPtr,KinBody::LinkConstPtr>, unsigned int > mCollidingLinksCount; // Counter for colliding links
     uint32_t numPlannerIterations; ///< number of planner iterations before failure

include/openrave/robot.h

@@ -303,20 +303,22 @@ class OPENRAVE_API RobotBase : public KinBody
         /// \param param The transformation of the end-effector in the global coord system
         /// \param solution Will be of size GetArmIndices().size() and contain the best solution
         /// \param[in] filteroptions A bitmask of \ref IkFilterOptions values controlling what is checked for each ik solution.
+        /// \return true if there is a valid ik solution.
         bool FindIKSolution(const IkParameterization& param, std::vector<dReal>& solution, int filteroptions) const;
         bool FindIKSolution(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, std::vector<dReal>& solution, int filteroptions) const;
-        bool FindIKSolution(const IkParameterization& param, int filteroptions, IkReturnPtr ikreturn) const;
-        bool FindIKSolution(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, int filteroptions, IkReturnPtr ikreturn) const;
+        bool FindIKSolution(const IkParameterization& param, int filteroptions, IkReturnPtr ikreturn, IkFailureAccumulatorPtr paccumulator = nullptr) const;
+        bool FindIKSolution(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, int filteroptions, IkReturnPtr ikreturn, IkFailureAccumulatorPtr paccumulator = nullptr) const;
 
         /// \brief Find all the IK solutions for the given end effector transform
         ///
         /// \param param The transformation of the end-effector in the global coord system
         /// \param solutions An array of all solutions, each element in solutions is of size GetArmIndices().size()
         /// \param[in] filteroptions A bitmask of \ref IkFilterOptions values controlling what is checked for each ik solution.
+        /// \return true if there is at least one valid ik solution.
         bool FindIKSolutions(const IkParameterization& param, std::vector<std::vector<dReal> >& solutions, int filteroptions) const;
         bool FindIKSolutions(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, std::vector<std::vector<dReal> >& solutions, int filteroptions) const;
-        bool FindIKSolutions(const IkParameterization& param, int filteroptions, std::vector<IkReturnPtr>& vikreturns) const;
-        bool FindIKSolutions(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, int filteroptions, std::vector<IkReturnPtr>& vikreturns) const;
+        bool FindIKSolutions(const IkParameterization& param, int filteroptions, std::vector<IkReturnPtr>& vikreturns, IkFailureAccumulatorPtr paccumulator = nullptr) const;
+        bool FindIKSolutions(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, int filteroptions, std::vector<IkReturnPtr>& vikreturns, IkFailureAccumulatorPtr paccumulator = nullptr) const;
 
         /** \brief returns the parameterization of a given IK type for the current manipulator position.