00001 #include "../include/limit_surface.h"
00002 #include "../include/debug.h"
00003 #include <iostream>
00004
00005 namespace ICR
00006 {
00007
00008
00009 LimitSurface::LimitSurface() : force_magnitude_(0), disc_(0), mu_0_(0) , mu_T_(0) {initializeSelectionMatrix();}
00010
00011 LimitSurface::LimitSurface(double force_magnitude) : force_magnitude_(force_magnitude), disc_(0),
00012 mu_0_(0) , mu_T_(0), contact_type_(Frictionless)
00013 {
00014 assert(force_magnitude >0);
00015 initializeSelectionMatrix();
00016
00017 addFrictionlessWrench();
00018 }
00019
00020 LimitSurface::LimitSurface(double force_magnitude, int disc,double mu_0) : force_magnitude_(force_magnitude), disc_(disc),
00021 mu_0_(mu_0), mu_T_(0), contact_type_(Frictional)
00022 {
00023 assert(force_magnitude >0);
00024 assert(mu_0_ > 0);
00025 assert(disc_ > 0);
00026
00027 initializeSelectionMatrix();
00028
00029 addFrictionlessWrench();
00030 addHardFingerWrenches();
00031 }
00032
00033 LimitSurface::LimitSurface(double force_magnitude, int disc, double mu_0,double mu_T) : force_magnitude_(force_magnitude), disc_(disc),
00034 mu_0_(mu_0), mu_T_(mu_T), contact_type_(Soft_Finger)
00035 {
00036 assert(mu_0_ > 0);
00037 assert(mu_T_ > 0);
00038 assert(disc_ > 0);
00039 assert(force_magnitude > 0);
00040
00041 initializeSelectionMatrix();
00042
00043 addFrictionlessWrench();
00044 addHardFingerWrenches();
00045 addSoftFingerWrenches();
00046 }
00047
00048 LimitSurface::LimitSurface(LimitSurface const& src) : force_magnitude_(src.force_magnitude_), disc_(src.disc_),
00049 mu_0_(src.mu_0_) , mu_T_(src.mu_T_),
00050 local_cone_(src.local_cone_), contact_type_(src.contact_type_), selection_matrix_(src.selection_matrix_) {}
00051
00052 LimitSurface& LimitSurface::operator=(LimitSurface const& src)
00053 {
00054 if (this !=&src)
00055 {
00056 force_magnitude_=src.force_magnitude_;
00057 mu_0_=src.mu_0_;
00058 mu_T_=src.mu_T_;
00059 disc_=src.disc_;
00060 local_cone_=src.local_cone_;
00061 contact_type_=src.contact_type_;
00062 selection_matrix_=src.selection_matrix_;
00063 }
00064 return *this;
00065 }
00066
00067 bool LimitSurface::operator==(LimitSurface const& other)const
00068 {
00069 if ((force_magnitude_==other.force_magnitude_) & (disc_==other.disc_) & (mu_0_==other.mu_0_)
00070 & (mu_T_==other.mu_T_) & (contact_type_==other.contact_type_) & (local_cone_.getNumPrimitiveWrenches()==other.local_cone_.getNumPrimitiveWrenches()))
00071 {
00072 if(*(local_cone_.getWrenches())==*(other.local_cone_.getWrenches()))
00073 return true;
00074 else
00075 return false;
00076 }
00077 else
00078 return false;
00079 }
00080
00081 LimitSurface::~LimitSurface(){}
00082
00083 void LimitSurface::initializeSelectionMatrix()
00084 {
00085
00086
00087 selection_matrix_.setZero();
00088 selection_matrix_.topLeftCorner(3,3) =Eigen::Matrix<double,3,3>::Identity(3,3);
00089 selection_matrix_(5,3)=1;
00090 }
00091
00092 void LimitSurface::addFrictionlessWrench()
00093 {
00094 Eigen::Vector4d gen_forces;
00095 gen_forces<< 0, 0,force_magnitude_, 0;
00096 local_cone_.addWrenches(selection_matrix_*gen_forces);
00097 }
00098
00099 void LimitSurface::addHardFingerWrenches()
00100 {
00101 Eigen::Matrix4Xd gen_forces=Eigen::Matrix4Xd::Zero(4,disc_);
00102
00103 Eigen::RowVectorXd lin=Eigen::RowVectorXd::LinSpaced(Eigen::Sequential,disc_+1,-PI,PI);
00104 lin.conservativeResize(disc_);
00105 double alpha=atan(mu_0_);
00106 double c_radius =sin(alpha)*force_magnitude_;
00107
00108 gen_forces.row(0)=lin.array().cos()*c_radius;
00109 gen_forces.row(1)=lin.array().sin()*c_radius;
00110 gen_forces.row(2).fill(force_magnitude_*cos(alpha));
00111 local_cone_.addWrenches(selection_matrix_*gen_forces);
00112 }
00113
00114 void LimitSurface::addSoftFingerWrenches()
00115 {
00116 Eigen::Matrix<double,4,2> gen_forces=Eigen::Matrix<double,4,2>::Zero(4,2);
00117
00118 gen_forces(2,0)=force_magnitude_;
00119 gen_forces(2,1)=force_magnitude_;
00120 gen_forces(3,0)=mu_T_*force_magnitude_;
00121 gen_forces(3,1)=-mu_T_*force_magnitude_;
00122
00123 local_cone_.addWrenches(selection_matrix_*gen_forces);
00124 }
00125
00126 int LimitSurface::getNumPrimitiveWrenches() const {return local_cone_.getNumPrimitiveWrenches();}
00127
00128 double LimitSurface::getMu0() const {return mu_0_;}
00129
00130 double LimitSurface::getMuT() const {return mu_T_;}
00131
00132 int LimitSurface::getDisc() const {return disc_;}
00133
00134 WrenchCone const* LimitSurface::getLocalWrenchCone() const {return &local_cone_;}
00135
00136 ContactType LimitSurface::getContactType() const {return contact_type_;}
00137
00138 double LimitSurface::getForceMagnitude() const {return force_magnitude_;}
00139
00140 std::ostream& operator<<(std::ostream& stream, LimitSurface const& lim_surf)
00141 {
00142 std::string contact_type;
00143
00144 if (lim_surf.contact_type_==Frictionless) contact_type="Frictionless";
00145 else if (lim_surf.contact_type_==Frictional) contact_type="Frictional";
00146 else if (lim_surf.contact_type_==Soft_Finger) contact_type="Soft Finger";
00147 else if (lim_surf.contact_type_==Undefined_CT) contact_type="Undefined";
00148 else contact_type="Warning in LimitSurface: Invalid contact type!";
00149
00150 stream <<'\n'<<"LIMIT SURFACE: "<<'\n'
00151 <<"Force magnitude: " << lim_surf.force_magnitude_ <<'\n'
00152 <<"Discretization: "<<lim_surf.disc_<<'\n'
00153 <<"Mu_0: "<<lim_surf.mu_0_<<'\n'
00154 <<"Mu_T: "<<lim_surf.mu_T_<<'\n'
00155 <<"Contact type: "<<contact_type<<'\n'<<'\n';
00156
00157 return stream;
00158 }
00159
00160
00161 }
00162