00001 #include "../include/wrench_space.h"
00002 #include "../include/debug.h"
00003 #include <iostream>
00004 #include <math.h>
00005 #include "assert.h"
00006
00007
00008 namespace ICR
00009 {
00010
00011
00012 WrenchSpace::WrenchSpace() : type_(Undefined_WS),full_dim_(false),contains_origin_(false),
00013 r_oc_insphere_(0),volume_(0),area_(0),dimension_(6){}
00014
00015 WrenchSpace::WrenchSpace(uint dimension) : type_(Undefined_WS),full_dim_(false),contains_origin_(false),
00016 r_oc_insphere_(0),volume_(0),area_(0),dimension_(dimension){}
00017
00018 WrenchSpace::WrenchSpace(const WrenchSpace& src) : type_(src.type_),full_dim_(src.full_dim_),
00019 contains_origin_(src.contains_origin_), r_oc_insphere_(src.r_oc_insphere_),
00020 volume_(src.volume_), area_(src.area_),dimension_(src.dimension_){}
00021
00022 WrenchSpace& WrenchSpace::operator=(const WrenchSpace& src)
00023 {
00024 if (this !=&src)
00025 {
00026 type_=src.type_;
00027 full_dim_=src.full_dim_;
00028 contains_origin_=src.contains_origin_;
00029 r_oc_insphere_=src.r_oc_insphere_;
00030 volume_=src.volume_;
00031 area_=src.area_;
00032 dimension_=src.dimension_;
00033 }
00034 return *this;
00035 }
00036
00037 WrenchSpace::~WrenchSpace() {}
00038
00039 WrenchSpaceType WrenchSpace::getWrenchSpaceType()const {return type_;}
00040
00041 bool WrenchSpace::isFullDimension()const{return full_dim_;}
00042
00043 bool WrenchSpace::containsOrigin()const{return contains_origin_;}
00044
00045 double WrenchSpace::getOcInsphereRadius()const{return r_oc_insphere_;}
00046
00047 double WrenchSpace::getVolume()const{return volume_;}
00048
00049 double WrenchSpace::getArea()const{return area_;}
00050
00051 uint WrenchSpace::getDimension()const{return dimension_;}
00052
00053
00054 SphericalWrenchSpace::SphericalWrenchSpace() : WrenchSpace(), radius_(0)
00055 {
00056 type_=Spherical;
00057 }
00058
00059 SphericalWrenchSpace::SphericalWrenchSpace(uint dimension,double radius) : WrenchSpace(dimension),radius_(radius)
00060 {
00061 assert(radius>0);
00062 assert(dimension > 1);
00063 type_=Spherical;
00064 full_dim_=true;
00065 contains_origin_=true;
00066 r_oc_insphere_=radius;
00067 computeVolume();
00068 computeArea();
00069 }
00070
00071 SphericalWrenchSpace::SphericalWrenchSpace(SphericalWrenchSpace const& src) : WrenchSpace(src), radius_(src.radius_){}
00072
00073 SphericalWrenchSpace& SphericalWrenchSpace::operator=(SphericalWrenchSpace const& src)
00074 {
00075 if (this !=&src)
00076 {
00077 WrenchSpace::operator=(src);
00078 radius_=src.radius_;
00079 }
00080 return *this;
00081 }
00082
00083 std::ostream& operator<<(std::ostream& stream, SphericalWrenchSpace const& s_wrench_space)
00084 {
00085 std::string wrench_space_type;
00086 if (s_wrench_space.type_==Spherical) wrench_space_type="Spherical";
00087 else wrench_space_type="Warning in SphericalWrenchSpace: Invalid rule type!";
00088
00089 stream <<'\n'<<"SPHERICAL WRENCH SPACE: "<<'\n'
00090 <<"Wrench space type: "<<wrench_space_type<<'\n'
00091 <<"Dimension: "<< s_wrench_space.dimension_<<'\n'
00092 <<"Contains origin: "<<s_wrench_space.contains_origin_<<'\n'
00093 <<"Has full dimension: "<<s_wrench_space.full_dim_<<'\n'
00094 <<"Radius: "<<s_wrench_space.radius_<<'\n'
00095 <<"Volume: "<<s_wrench_space.volume_<<'\n'
00096 <<"Area: "<<s_wrench_space.area_<<'\n'
00097 <<"Origin-centered insphere radius: "<<s_wrench_space.r_oc_insphere_<<'\n'<<'\n';
00098
00099 return stream;
00100 }
00101
00102 SphericalWrenchSpace::~SphericalWrenchSpace(){}
00103
00104 void SphericalWrenchSpace::computeArea()
00105 {
00106 double C_n;
00107
00108 if ( dimension_ % 2== 0 )
00109 C_n=pow(PI,dimension_/2)/factorial(dimension_/2);
00110 else
00111 C_n=pow(2,(dimension_+1)/2)*pow(PI,(dimension_-1)/2)/dfactorial(dimension_);
00112
00113 area_=dimension_*C_n*pow(radius_,dimension_-1);
00114 }
00115
00116 void SphericalWrenchSpace::computeVolume()
00117 {
00118 double C_n;
00119
00120 if ( dimension_ % 2== 0 )
00121 C_n=pow(PI,dimension_/2)/factorial(dimension_/2);
00122 else
00123 C_n=pow(2,(dimension_+1)/2)*pow(PI,(dimension_-1)/2)/dfactorial(dimension_);
00124
00125 volume_=C_n*pow(radius_,dimension_);
00126 }
00127
00128 void SphericalWrenchSpace::setRadius(double const radius)
00129 {
00130 assert(radius >0);
00131 radius_=radius;
00132 computeVolume();
00133 computeArea();
00134
00135
00136 r_oc_insphere_=radius;
00137 contains_origin_=true;
00138 }
00139 double SphericalWrenchSpace::getRadius()const{return radius_;}
00140
00141
00142 DiscreteWrenchSpace::DiscreteWrenchSpace() : ch_computed_(false),num_wrenches_(0),num_vtx_(0),num_facets_(0)
00143 {
00144 type_=Discrete;
00145 }
00146
00147 DiscreteWrenchSpace::DiscreteWrenchSpace(DiscreteWrenchSpace const& src) : WrenchSpace(src),ch_computed_(src.ch_computed_),
00148 conv_hull_(src.conv_hull_),num_wrenches_(src.num_wrenches_),
00149 num_vtx_(src.num_vtx_),num_facets_(src.num_facets_){}
00150
00151 DiscreteWrenchSpace& DiscreteWrenchSpace::operator=(DiscreteWrenchSpace const& src)
00152 {
00153 if (this !=&src)
00154 {
00155 WrenchSpace::operator=(src);
00156 ch_computed_=src.ch_computed_;
00157 conv_hull_=src.conv_hull_;
00158 num_wrenches_=src.num_wrenches_;
00159 num_vtx_=src.num_vtx_;
00160 num_facets_=src.num_facets_;
00161 }
00162 return *this;
00163 }
00164
00165 std::ostream& operator<<(std::ostream& stream, DiscreteWrenchSpace const& d_wrench_space)
00166 {
00167 std::string wrench_space_type;
00168 if (d_wrench_space.type_==Discrete) wrench_space_type="Discrete";
00169 else wrench_space_type="Warning in DiscreteWrenchSpace: Invalid rule type!";
00170
00171 stream <<'\n'<<"DISCRETE WRENCH SPACE: "<<'\n'
00172 <<"Wrench space type: "<<wrench_space_type<<'\n'
00173 <<"Convex hull computed: "<<d_wrench_space.ch_computed_<<'\n'
00174 <<"Contains origin: "<<d_wrench_space.contains_origin_<<'\n'
00175 <<"Has full dimension: "<<d_wrench_space.full_dim_<<'\n'
00176 <<"Origin-centered insphere radius: "<<d_wrench_space.r_oc_insphere_<<'\n'
00177 <<"Volume: "<<d_wrench_space.volume_<<'\n'
00178 <<"Area: "<<d_wrench_space.area_<<'\n'
00179 <<"Number of input wrenches: "<<d_wrench_space.num_wrenches_<<'\n'
00180 <<"Number of vertices: "<<d_wrench_space.num_vtx_<<'\n'
00181 <<"Number of facets: "<<d_wrench_space.num_facets_<<'\n'<<'\n';
00182
00183 return stream;
00184 }
00185
00186 DiscreteWrenchSpace::~DiscreteWrenchSpace(){}
00187
00188 bool DiscreteWrenchSpace::convHullComputed()const{return ch_computed_;}
00189
00190 orgQhull::Qhull const* DiscreteWrenchSpace::getConvexHull()const
00191 {
00192 assert(ch_computed_);
00193 return &conv_hull_;
00194 }
00195
00196 void DiscreteWrenchSpace::computeConvexHull(double const* wrenches,uint num_wrenches)
00197 {
00198 assert(wrenches != NULL);
00199 assert(num_wrenches > dimension_);
00200 num_wrenches_=num_wrenches;
00201
00202 try{
00203 conv_hull_.runQhull("", dimension_,num_wrenches,wrenches ,"Qx Qt");
00204 }
00205 catch(std::exception& exc)
00206 {
00207 #ifdef DEBUG_QHULL
00208 std::cout<<exc.what()<<std::endl;
00209 #endif
00210 contains_origin_=false;
00211 full_dim_=false;
00212 ch_computed_=true;
00213 return;
00214 }
00215
00216 conv_hull_.defineVertexNeighborFacets();
00217 area_=conv_hull_.area();
00218 volume_=conv_hull_.volume();
00219 num_vtx_=conv_hull_.vertexCount();
00220 num_facets_=conv_hull_.facetCount();
00221
00222 facetT* curr_f=conv_hull_.beginFacet().getFacetT();
00223 r_oc_insphere_=-(curr_f->offset);
00224
00225 #ifdef DEBUG_DISCRETEWRENCHSPACE // File for writing the hyperplanes to a file to compare in Matlab
00226 remove("../debug/hyperplanes.txt");
00227 FILE* hp=fopen ("../debug/hyperplanes.txt","a");
00228 if(!hp)
00229 std::cout<<"Warning in DiscreteWrenchSpace: Couldn't write to file."<<std::endl;
00230 #endif
00231
00232 for(uint i=0;i< num_facets_;i++)
00233 {
00234 #ifdef DEBUG_DISCRETEWRENCHSPACE //Write hyperplanes to file
00235 fprintf(hp, "% f % f % f % f % f % f %f \n",(curr_f->normal)[0],(curr_f->normal)[1],(curr_f->normal)[2],(curr_f->normal)[3],(curr_f->normal)[4],(curr_f->normal)[5],curr_f->offset);
00236 #endif
00237 curr_f->id=i;
00238 r_oc_insphere_ = (-(curr_f->offset) < r_oc_insphere_) ? -(curr_f->offset) : r_oc_insphere_;
00239 curr_f=curr_f->next;
00240 }
00241
00242 contains_origin_=(r_oc_insphere_ > EPSILON_FORCE_CLOSURE) ? true : false;
00243 full_dim_=true;
00244 ch_computed_=true;
00245
00246 #ifdef DEBUG_DISCRETEWRENCHSPACE
00247 fclose (hp);
00248 #endif
00249 }
00250
00251 uint DiscreteWrenchSpace::getNumWrenches()const{return num_wrenches_;}
00252
00253 uint DiscreteWrenchSpace::getNumVertices()const{return num_vtx_;}
00254
00255 uint DiscreteWrenchSpace::getNumFacets()const{return num_facets_;}
00256
00257
00258 }