gaussian.cc 5.11 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
// Copyright (c) 2018, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-core.
// hpp-core 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-core 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-core. If not, see <http://www.gnu.org/licenses/>.

#include <hpp/core/configuration-shooter/gaussian.hh>

#include <math.h>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>

#include <pinocchio/algorithm/joint-configuration.hpp>

#include <pinocchio/multibody/model.hpp>

#include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/liegroup.hh>

namespace hpp {
  namespace core {
    namespace configurationShooter {
      namespace liegroup = pinocchio::liegroup;

      template <typename LG1, typename LG2>
      void computeSigmasAlgo (liegroup::CartesianProductOperation<LG1, LG2> lg,
            vectorOut_t sigmas, vectorIn_t upper, vectorIn_t lower);

      template <int N>
      void computeSigmasAlgo (liegroup::SpecialOrthogonalOperation<N>,
            vectorOut_t sigmas, vectorIn_t, vectorIn_t)
      {
        sigmas.setConstant (2*M_PI/std::sqrt((value_type)N));
      }

      template <int N>
      void computeSigmasAlgo (liegroup::SpecialEuclideanOperation<N>,
            vectorOut_t sigmas, vectorIn_t upper, vectorIn_t lower)
      {
        typedef liegroup::CartesianProductOperation<
          liegroup::VectorSpaceOperation<N,true>,
          liegroup::SpecialOrthogonalOperation<N>
            > LG_t;
        computeSigmasAlgo (LG_t(), sigmas, upper, lower);
      }

      template <int N, bool rot>
      void computeSigmasAlgo (liegroup::VectorSpaceOperation<N, rot>,
            vectorOut_t sigmas, vectorIn_t upper, vectorIn_t lower)
      {
        // TODO isFinite was added after 3.2.4
        // sigmas.array() =
          // (upper.array().isFinite() && lower.array().isFinite())
          // .select (upper - lower, 1);
        for (size_type i = 0; i < sigmas.size(); ++i) {
          if (Eigen::numext::isfinite (upper(i))
              && Eigen::numext::isfinite (lower(i)))
            sigmas(i) = upper(i)-lower(i);
          else
            sigmas(i) = 1.;
        }
      }

      template <typename LG1, typename LG2>
      void computeSigmasAlgo (liegroup::CartesianProductOperation<LG1, LG2>,
            vectorOut_t sigmas, vectorIn_t upper, vectorIn_t lower)
      {
        computeSigmasAlgo (LG1(), sigmas.head(LG1::NV), upper.head(LG1::NQ), lower.head(LG1::NQ));
        computeSigmasAlgo (LG2(), sigmas.tail(LG2::NV), upper.tail(LG2::NQ), lower.tail(LG2::NQ));
      }

      struct ComputeSigmasStep : public se3::fusion::JointModelVisitor<ComputeSigmasStep>
      {
        typedef boost::fusion::vector<const se3::Model&, vector_t&> ArgsType;

        JOINT_MODEL_VISITOR_INIT(ComputeSigmasStep);

        template<typename JointModel>
          static void algo(const se3::JointModelBase<JointModel> & jmodel,
              const se3::Model& model,
              vector_t& sigmas)
          {
            typedef typename pinocchio::DefaultLieGroupMap::operation<JointModel>::type LG_t;
            computeSigmasAlgo (LG_t(), 
                jmodel.jointVelocitySelector (sigmas),
                jmodel.jointConfigSelector   (model.upperPositionLimit),
                jmodel.jointConfigSelector   (model.lowerPositionLimit));
          }
      };

      template<>
      void ComputeSigmasStep::algo<se3::JointModelComposite>(const se3::JointModelBase<se3::JointModelComposite> & jmodel,
              const se3::Model& model,
              vector_t& sigmas)
      {
        se3::details::Dispatch<ComputeSigmasStep>::run(jmodel, ComputeSigmasStep::ArgsType(model, sigmas));
      }

      ConfigurationPtr_t Gaussian::shoot () const
      {
112
        static boost::random::mt19937 eng;
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
        vector_t velocity (robot_->numberDof());
        for (size_type i = 0; i < velocity.size(); ++i)
        {
          boost::random::normal_distribution<value_type> distrib(0, sigmas_[i]);
          velocity[i] = distrib (eng);
        }

        ConfigurationPtr_t config(new Configuration_t(robot_->configSize ()));
        ::hpp::pinocchio::integrate (robot_, center_, velocity, *config);
        ::hpp::pinocchio::saturate  (robot_, *config);

        return config;
      }

      void Gaussian::sigma(const value_type& factor)
      {
        const se3::Model& model = robot_->model();
        ComputeSigmasStep::ArgsType args (model, sigmas_);
        for(std::size_t i = 1; i < model.joints.size(); ++i)
          ComputeSigmasStep::run (model.joints[i], args);

        sigmas_ *= factor;
      }
    } //   namespace configurationShooter
  } //   namespace core
} // namespace hpp