ConfigurationSpace.cpp
Go to the documentation of this file.
1 // -*- mode: c++; indent-tabs-mode: nil; c-basic-offset: 4; tab-width: 4; -*-
2 #include "Configuration.h"
3 #include "ConfigurationSpace.h"
4 #define _USE_MATH_DEFINES // for MSVC
5 #include <math.h>
6 #include <stdlib.h>
7 
8 using namespace PathEngine;
9 
10 ConfigurationSpace::ConfigurationSpace(unsigned int i_size) : m_size(i_size)
11 {
12  m_weights.resize(m_size);
13  m_ubounds.resize(m_size);
14  m_lbounds.resize(m_size);
16 
17  for (unsigned int i=0; i<m_size; i++){
18  m_weights[i] = 1.0;
19  m_ubounds[i] = m_lbounds[i] = 0.0;
20  m_isUnboundedRotation[i] = false;
21  }
22 }
23 
25 {
26  if (m_size != cfg.size()) return false;
27 
28  for (unsigned int i=0; i<m_size; i++){
29  if (!weight(i)) continue;
30  double v = cfg[i];
31  if (!m_isUnboundedRotation[i]){
32  if (v > m_ubounds[i] || v < m_lbounds[i]) return false;
33  }
34  }
35  return true;
36 }
37 
38 double &ConfigurationSpace::weight(unsigned int i_rank)
39 {
40  return m_weights[i_rank];
41 }
42 
43 const double ConfigurationSpace::weight(unsigned int i_rank) const
44 {
45  return m_weights[i_rank];
46 }
47 
48 void ConfigurationSpace::bounds(unsigned int i_rank, double min, double max)
49 {
50  m_ubounds[i_rank] = max;
51  m_lbounds[i_rank] = min;
52 }
53 
54 double& ConfigurationSpace::lbound(unsigned int i_rank)
55 {
56  return m_lbounds[i_rank];
57 }
58 
59 double& ConfigurationSpace::ubound(unsigned int i_rank)
60 {
61  return m_ubounds[i_rank];
62 }
63 
65 {
66  Configuration cfg(m_size);
67  for (unsigned int i=0; i<m_size; i++){
69  cfg.value(i) = (rand()/(double)RAND_MAX) * 2 * M_PI;
70  }else{
71  double delta = m_ubounds[i] - m_lbounds[i];
72  cfg.value(i) = (rand()/(double)RAND_MAX) * delta + m_lbounds[i];
73  }
74  }
75  return cfg;
76 }
77 
79 {
80  return m_size;
81 }
82 
83 void ConfigurationSpace::unboundedRotation(unsigned int i_rank, bool i_flag)
84 {
85  m_isUnboundedRotation[i_rank] = i_flag;
86 }
87 
88 bool ConfigurationSpace::unboundedRotation(unsigned int i_rank)
89 {
90  return m_isUnboundedRotation[i_rank];
91 }
unsigned int size()
get the number of degrees of freedom
Configuration random()
generate random position
std::vector< double > m_lbounds
std::vector< double > m_ubounds
static int min(int a, int b)
bool isValid(const Configuration &cfg) const
全ての要素が有効な範囲内にあるかどうか検査する
png_uint_32 i
Definition: png.h:2735
ConfigurationSpace(unsigned int i_size)
constructor
void unboundedRotation(unsigned int i_rank, bool i_flag)
specify i th degree of freedom is unbounded rotaion or not. default is false
std::vector< bool > m_isUnboundedRotation
std::vector< double > m_weights
double & weight(unsigned int i_rank)
get weight for i_rank th element
void bounds(unsigned int i_rank, double min, double max)
set bounds for i_rank th element
#define M_PI
const double value(unsigned int i_rank) const
double & lbound(unsigned int i_rank)
get lower bound of i_rank th element
double & ubound(unsigned int i_rank)
get upper bound for i_rank th element
static int max(int a, int b)
unsigned int size() const
get the number of degrees of freedom


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:02