rbprmbuilder.impl.hh 7.98 KB
Newer Older
Steve Tonneau's avatar
Steve Tonneau committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
// Copyright (c) 2014 CNRS
// Author: Florent Lamiraux
//
// This file is part of hpp-manipulation-corba.
// hpp-manipulation-corba is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-manipulation-corba is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-manipulation-corba.  If not, see
// <http://www.gnu.org/licenses/>.

#ifndef HPP_RBPRM_CORBA_BUILDER_IMPL_HH
# define HPP_RBPRM_CORBA_BUILDER_IMPL_HH

# include <hpp/core/problem-solver.hh>
# include "rbprmbuilder.hh"
Steve Tonneau's avatar
Steve Tonneau committed
23
# include <hpp/rbprm/rbprm-device.hh>
Steve Tonneau's avatar
Steve Tonneau committed
24
# include <hpp/rbprm/rbprm-fullbody.hh>
Steve Tonneau's avatar
Steve Tonneau committed
25
26
# include <hpp/rbprm/rbprm-shooter.hh>
# include <hpp/rbprm/rbprm-validation.hh>
27
# include <hpp/rbprm/sampling/analysis.hh>
28
29
30
31
# include <hpp/core/collision-path-validation-report.hh>
# include <hpp/core/problem-solver.hh>
# include <hpp/core/discretized-collision-checking.hh>
# include <hpp/core/straight-path.hh>
Steve Tonneau's avatar
Steve Tonneau committed
32
33
34
35
36
37

namespace hpp {
  namespace rbprm {
    namespace impl {
      using CORBA::Short;

Steve Tonneau's avatar
Steve Tonneau committed
38
39
40
    struct BindShooter
    {
        BindShooter(const std::size_t shootLimit = 10000,
Steve Tonneau's avatar
Steve Tonneau committed
41
                    const std::size_t displacementLimit = 1000)
Steve Tonneau's avatar
Steve Tonneau committed
42
43
44
45
46
47
            : shootLimit_(shootLimit)
            , displacementLimit_(displacementLimit) {}

        hpp::rbprm::RbPrmShooterPtr_t create (const hpp::model::DevicePtr_t& robot)
        {
            hpp::model::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::model::RbPrmDevice>(robot);
Steve Tonneau's avatar
Steve Tonneau committed
48
            rbprm::RbPrmShooterPtr_t shooter = hpp::rbprm::RbPrmShooter::create
49
                    (robotcast,problemSolver_->problem ()->collisionObstacles(),romFilter_,normalFilter_,shootLimit_,displacementLimit_);
Steve Tonneau's avatar
Steve Tonneau committed
50
51
52
            if(!so3Bounds_.empty())
                shooter->BoundSO3(so3Bounds_);
            return shooter;
Steve Tonneau's avatar
Steve Tonneau committed
53
        }
54
55
56
57

        hpp::core::PathValidationPtr_t createPathValidation (const hpp::model::DevicePtr_t& robot, const hpp::model::value_type& val)
        {
            hpp::model::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::model::RbPrmDevice>(robot);
58
            hpp::rbprm::RbPrmValidationPtr_t validation(hpp::rbprm::RbPrmValidation::create(robotcast, romFilter_, normalFilter_));
59
60
61
            hpp::core::DiscretizedCollisionCheckingPtr_t collisionChecking = hpp::core::DiscretizedCollisionChecking::create(robot,val);
            collisionChecking->add (validation);
            return collisionChecking;
62
63
        }

Steve Tonneau's avatar
Steve Tonneau committed
64
        hpp::core::ProblemSolverPtr_t problemSolver_;
Steve Tonneau's avatar
Steve Tonneau committed
65
        std::vector<std::string> romFilter_;
66
        std::map<std::string, NormalFilter> normalFilter_;
Steve Tonneau's avatar
Steve Tonneau committed
67
68
        std::size_t shootLimit_;
        std::size_t displacementLimit_;
Steve Tonneau's avatar
Steve Tonneau committed
69
        std::vector<double> so3Bounds_;
Steve Tonneau's avatar
Steve Tonneau committed
70
71
    };

Steve Tonneau's avatar
Steve Tonneau committed
72
73
74
      class RbprmBuilder : public virtual POA_hpp::corbaserver::rbprm::RbprmBuilder
      {
        public:
Steve Tonneau's avatar
Steve Tonneau committed
75
        RbprmBuilder ();
Steve Tonneau's avatar
Steve Tonneau committed
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90

        virtual void loadRobotRomModel (const char* robotName,
                 const char* rootJointType,
                 const char* packageName,
                 const char* modelName,
                 const char* urdfSuffix,
                 const char* srdfSuffix) throw (hpp::Error);

        virtual void loadRobotCompleteModel (const char* robotName,
                 const char* rootJointType,
                 const char* packageName,
                 const char* modelName,
                 const char* urdfSuffix,
                 const char* srdfSuffix) throw (hpp::Error);

Steve Tonneau's avatar
Steve Tonneau committed
91
92
93
94
95
96
97
98

        virtual void loadFullBodyRobot (const char* robotName,
                 const char* rootJointType,
                 const char* packageName,
                 const char* modelName,
                 const char* urdfSuffix,
                 const char* srdfSuffix) throw (hpp::Error);

Steve Tonneau's avatar
Steve Tonneau committed
99
        virtual void setFilter(const hpp::Names_t& roms) throw (hpp::Error);
100
        virtual void setNormalFilter(const char* romName, const hpp::floatSeq& normal, double range) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
101
102
        virtual void boundSO3(const hpp::floatSeq& limitszyx) throw (hpp::Error);

Steve Tonneau's avatar
Steve Tonneau committed
103

Steve Tonneau's avatar
Steve Tonneau committed
104
        virtual hpp::floatSeq* getSampleConfig(const char* limb, unsigned short sampleId) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
105
        virtual hpp::floatSeq* getSamplePosition(const char* limb, unsigned short sampleId) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
106
        virtual CORBA::UShort getNumSamples(const char* limb) throw (hpp::Error);
107
        virtual hpp::floatSeq* getOctreeNodeIds(const char* limb) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
108
        virtual double getSampleValue(const char* limb, const char* valueName, unsigned short sampleId) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
109

Steve Tonneau's avatar
Steve Tonneau committed
110
111
112
        virtual hpp::floatSeq* generateContacts(const hpp::floatSeq& configuration,
                                                const hpp::floatSeq& direction) throw (hpp::Error);

113
114
115
116
        virtual hpp::floatSeq* getContactSamplesIds(const char* limb,
                                                   const hpp::floatSeq& configuration,
                                                   const hpp::floatSeq& direction) throw (hpp::Error);

117
118
119
        virtual hpp::floatSeq* getSamplesIdsInOctreeNode(const char* limb,
                                                   double octreeNodeId) throw (hpp::Error);

120
        virtual void addLimb(const char* id, const char* limb, const char* effector, const hpp::floatSeq& offset, const hpp::floatSeq& normal, double x, double y,
121
                             unsigned short samples, const char *heuristicName, double resolution, const char *contactType) throw (hpp::Error);
122
        virtual void addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName, double loadValues) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
123

Steve Tonneau's avatar
Steve Tonneau committed
124
125
        virtual void setStartState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
        virtual void setEndState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
126
127
        virtual hpp::floatSeqSeq* interpolate(double timestep, double path, double robustnessTreshold) throw (hpp::Error);
        virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
128
        virtual void saveComputedStates(const char* filepath) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
129
        virtual void saveLimbDatabase(const char* limbname,const char* filepath) throw (hpp::Error);
130
131
        virtual hpp::floatSeq* getOctreeBox(const char* limbName, double sampleId) throw (hpp::Error);
        virtual hpp::floatSeqSeq* getOctreeBoxes(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);
132
        virtual hpp::floatSeq* getOctreeTransform(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);
133
        virtual CORBA::Short isConfigBalanced(const hpp::floatSeq& config, const hpp::Names_t& contactLimbs, double robustnessTreshold) throw (hpp::Error);
134
135
136
        virtual void runSampleAnalysis(const char* analysis, double isstatic) throw (hpp::Error);
        virtual void runLimbSampleAnalysis(const char* limbname, const char* analysis, double isstatic) throw (hpp::Error);

Steve Tonneau's avatar
Steve Tonneau committed
137
        public:
Steve Tonneau's avatar
Steve Tonneau committed
138
139
140
        void SetProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver);

        private:
Steve Tonneau's avatar
Steve Tonneau committed
141
142
143
144
        /// \brief Pointer to hppPlanner object of hpp::corbaServer::Server.
        core::ProblemSolverPtr_t problemSolver_;

        private:
Steve Tonneau's avatar
Steve Tonneau committed
145
        model::T_Rom romDevices_;
Steve Tonneau's avatar
Steve Tonneau committed
146
        rbprm::RbPrmFullBodyPtr_t fullBody_;
Steve Tonneau's avatar
Steve Tonneau committed
147
        bool romLoaded_;
Steve Tonneau's avatar
Steve Tonneau committed
148
        bool fullBodyLoaded_;
Steve Tonneau's avatar
Steve Tonneau committed
149
        BindShooter bindShooter_;
Steve Tonneau's avatar
Steve Tonneau committed
150
151
        rbprm::State startState_;
        rbprm::State endState_;
Steve Tonneau's avatar
Steve Tonneau committed
152
        std::vector<rbprm::State> lastStatesComputed_;
Steve Tonneau's avatar
Steve Tonneau committed
153
        sampling::AnalysisFactory* analysisFactory_;
Steve Tonneau's avatar
Steve Tonneau committed
154
155
156
157
158
159
      }; // class RobotBuilder
    } // namespace impl
  } // namespace manipulation
} // namespace hpp

#endif // HPP_RBPRM_CORBA_BUILDER_IMPL_HH