Go to the documentation of this file.00001 #include "../include/wrench_cone.h"
00002 #include "../include/debug.h"
00003
00004 namespace ICR
00005 {
00006
00007
00008 WrenchCone::WrenchCone() : num_primitive_wrenches_(0),id_(0) {}
00009
00010 WrenchCone::WrenchCone(uint id) :num_primitive_wrenches_(0), id_(id){}
00011
00012 WrenchCone::WrenchCone(uint id, Eigen::Matrix<double,6,Eigen::Dynamic> const& cone) : num_primitive_wrenches_(cone.cols()), cone_(cone),id_(id) {}
00013
00014 WrenchCone::WrenchCone(WrenchCone const& src) : num_primitive_wrenches_(src.num_primitive_wrenches_), cone_(src.cone_) ,id_(src.id_){}
00015
00016 WrenchCone& WrenchCone::operator=(WrenchCone const& src)
00017 {
00018 if (this !=&src)
00019 {
00020 num_primitive_wrenches_=src.num_primitive_wrenches_;
00021 cone_=src.cone_;
00022 id_=src.id_;
00023 }
00024 return *this;
00025 }
00026
00027 std::ostream& operator<<(std::ostream& stream,WrenchCone const& wrench_cone)
00028 {
00029
00030 stream <<'\n'<<"WRENCH CONE: "<<'\n'
00031 <<"Id: "<<wrench_cone.id_<<'\n'
00032 <<"Number of primitive wrenches: " << wrench_cone.num_primitive_wrenches_<<'\n'
00033 <<"Primitive wrenches: " <<'\n'<< wrench_cone.cone_<<'\n'<<'\n';
00034
00035 return stream;
00036 }
00037
00038 WrenchCone::~WrenchCone() {}
00039
00040 Eigen::Matrix<double,6,Eigen::Dynamic> const* WrenchCone::getWrenches()const {return &cone_;}
00041
00042 Eigen::Matrix<double,6,Eigen::Dynamic>* WrenchCone::getWrenches(){return &cone_;}
00043
00044 uint WrenchCone::getNumPrimitiveWrenches() const {return num_primitive_wrenches_;}
00045
00046 void WrenchCone::scaleWrenches(double scale)
00047 {
00048 cone_=cone_*scale;
00049 }
00050
00051 void WrenchCone::scaleWrenchTorques(double scale)
00052 {
00053 cone_.block(3, 0, 3, cone_.cols())=cone_.block(3, 0, 3, cone_.cols())*scale;
00054 }
00055
00056 void WrenchCone::addWrenches(Eigen::Matrix<double,6,Eigen::Dynamic> const& wrenches)
00057 {
00058 cone_.conservativeResize(6,cone_.cols()+wrenches.cols());
00059 cone_.block(0,cone_.cols()-wrenches.cols(), 6,wrenches.cols())=wrenches;
00060 num_primitive_wrenches_=cone_.cols();
00061 }
00062
00063 void WrenchCone::setWrenches(Eigen::Matrix<double,6,Eigen::Dynamic> const& wrenches)
00064 {
00065 cone_=wrenches;
00066 num_primitive_wrenches_=cone_.cols();
00067 }
00068
00069 void WrenchCone::setId(uint id){id_=id;}
00070
00071 uint WrenchCone::getId()const{return id_;}
00072
00073
00074 }