ConfigurationSpace.cpp
Go to the documentation of this file.
00001 // -*- mode: c++; indent-tabs-mode: nil; c-basic-offset: 4; tab-width: 4; -*-
00002 #include "Configuration.h"
00003 #include "ConfigurationSpace.h"
00004 #define _USE_MATH_DEFINES // for MSVC
00005 #include <math.h>
00006 #include <stdlib.h>
00007 
00008 using namespace PathEngine;
00009 
00010 ConfigurationSpace::ConfigurationSpace(unsigned int i_size) : m_size(i_size)
00011 {
00012     m_weights.resize(m_size);
00013     m_ubounds.resize(m_size);
00014     m_lbounds.resize(m_size);
00015     m_isUnboundedRotation.resize(m_size);
00016     
00017     for (unsigned int i=0; i<m_size; i++){
00018         m_weights[i] = 1.0;
00019         m_ubounds[i] = m_lbounds[i] = 0.0;
00020         m_isUnboundedRotation[i] = false;
00021     }
00022 }
00023 
00024 bool ConfigurationSpace::isValid(const Configuration &cfg) const
00025 {
00026     if (m_size != cfg.size()) return false;
00027 
00028     for (unsigned int i=0; i<m_size; i++){
00029         if (!weight(i)) continue;
00030         double v = cfg[i];
00031         if (!m_isUnboundedRotation[i]){
00032             if (v > m_ubounds[i] || v < m_lbounds[i]) return false;
00033         }
00034     }
00035     return true;
00036 }
00037 
00038 double &ConfigurationSpace::weight(unsigned int i_rank)
00039 {
00040     return m_weights[i_rank];
00041 }
00042 
00043 const double ConfigurationSpace::weight(unsigned int i_rank) const
00044 {
00045     return m_weights[i_rank];
00046 }
00047 
00048 void ConfigurationSpace::bounds(unsigned int i_rank, double min, double max)
00049 {
00050     m_ubounds[i_rank] = max;
00051     m_lbounds[i_rank] = min;
00052 }
00053 
00054 double& ConfigurationSpace::lbound(unsigned int i_rank)
00055 {
00056     return m_lbounds[i_rank];
00057 }
00058 
00059 double& ConfigurationSpace::ubound(unsigned int i_rank)
00060 {
00061     return m_ubounds[i_rank];
00062 }
00063 
00064 Configuration ConfigurationSpace::random()
00065 {
00066     Configuration cfg(m_size);
00067     for (unsigned int i=0; i<m_size; i++){
00068         if (m_isUnboundedRotation[i]){
00069             cfg.value(i) = (rand()/(double)RAND_MAX) * 2 * M_PI;
00070         }else{
00071             double delta = m_ubounds[i] - m_lbounds[i];
00072             cfg.value(i) = (rand()/(double)RAND_MAX) * delta + m_lbounds[i];
00073         }
00074     }
00075     return cfg;
00076 }
00077 
00078 unsigned int ConfigurationSpace::size()
00079 {
00080     return m_size;
00081 }
00082 
00083 void ConfigurationSpace::unboundedRotation(unsigned int i_rank, bool i_flag)
00084 {
00085     m_isUnboundedRotation[i_rank] = i_flag;
00086 }
00087 
00088 bool ConfigurationSpace::unboundedRotation(unsigned int i_rank)
00089 {
00090     return m_isUnboundedRotation[i_rank];
00091 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:53