diff --git a/src/base/init.cpp b/src/base/init.cpp index 4d1f0a6..94a2b67 100644 --- a/src/base/init.cpp +++ b/src/base/init.cpp @@ -46,6 +46,7 @@ #include "../simulation/puma560sim.h" #include "../simulation/aseairb2000sim.h" #include "../simulation/quadrotorsim.h" +#include "../simulation/euitibotsim.h" #include "../world/world.h" namespace mr @@ -70,6 +71,7 @@ void mrcoreInit() Puma560Sim puma; QuadrotorSim quad; AseaIRB2000Sim asea; + EuitiBotSim euito; World world; diff --git a/src/base/object.cpp b/src/base/object.cpp index 5b69460..2791343 100644 --- a/src/base/object.cpp +++ b/src/base/object.cpp @@ -49,4 +49,6 @@ namespace mr return (*ptrCreateObject)(); return 0; } + void Object::writeToXML(XMLElement* parent){} + void Object::readFromXML(XMLElement* parent){} } //mr \ No newline at end of file diff --git a/src/base/object.h b/src/base/object.h index ad5fa2b..94a42a7 100644 --- a/src/base/object.h +++ b/src/base/object.h @@ -34,13 +34,16 @@ #include #include #include -#include "stream.h" +#include "streamstring.h" +//#include "xml.h" + using namespace std; using namespace mr; namespace mr { + class XMLElement; /*! \class Object \brief Object, the base class that supports RTTI and serialization. @@ -54,6 +57,8 @@ class Object virtual void writeToStream(Stream& stream)=0; virtual void readFromStream(Stream& stream)=0; + virtual void writeToXML(XMLElement* parent); + virtual void readFromXML(XMLElement* parent); protected: ///The map is a singleton, static accessed by this method diff --git a/src/base/streamstring.h b/src/base/streamstring.h index cb7fbf8..111fb4d 100644 --- a/src/base/streamstring.h +++ b/src/base/streamstring.h @@ -37,6 +37,7 @@ #include #include "stream.h" + using namespace std; @@ -66,7 +67,6 @@ class StreamString: public Stream { return ((stringstream*)stream)->str(); } - protected: //non-copyable, non movable StreamString(const StreamString& str); diff --git a/src/base/xml.cpp b/src/base/xml.cpp index 4c4d28a..aba4b3a 100644 --- a/src/base/xml.cpp +++ b/src/base/xml.cpp @@ -5625,10 +5625,32 @@ void XMLElement :: RemoveAllContents() contentsnum = 0; } +//unsigned int XMLElement :: GetContents(XMLContent** x) +// { +// //return contents; +// int C = 0; +//#ifdef XML_USE_STL +// for(unsigned int i = 0 ; i < children.size() && deep != 0 ; i++) +// { +// XMLContent* ch = &contents[i]; +//#else +// for(unsigned int i = 0 ; i < contentsnum; i++) +// { +// if (!contents[i]) +// continue; +// XMLContent* ch = contents[i]; +//#endif +// C += ch->GetContents(x + C); +// x[C++] = ch; +// } +// +// return C; +// } + XMLContent** XMLElement :: GetContents() - { - return contents; - } +{ +return contents; +} unsigned int XMLElement :: GetContentsNum() { @@ -5942,8 +5964,20 @@ size_t XMLContent :: GetValue(char* x,int NoDecode) const #endif return strlen(x); } - - +/////////////////////////////////////// PROYECTO FRAN //////////////////////////////////// +//int XMLContent::GetValueCad (char* cad) +// { +// char t[50] = {0}; +// GetValue(t); +// strcpy(cad,t); +// return strlen(cad); +// } +size_t XMLContent ::GetSize () +{ + return strlen(c); +} + +////////////////////////////////////////////////////////////////////////////////////////////// void XMLContent :: SetValue(const char* VV,int NoDecode) { #ifdef XML_USE_STL @@ -6581,6 +6615,7 @@ float XMLVariable :: GetValueFloat() return (float)atof(d); } + void XMLVariable :: SetFormattedValue(const char* fmt,...) { va_list args; @@ -6930,6 +6965,8 @@ int XMLElement :: XMLQuery(const char* expression2,XMLElement** rv,unsigned int + + // Global functions Z* XML :: ReadToZ(const char* file,XMLTransform* eclass,class XMLTransformData* edata,bool IsU) { @@ -7424,6 +7461,7 @@ int XMLGetStringW(const char* section,const char* Tattr,const wchar_t* defv,wcha #endif + }; diff --git a/src/base/xml.h b/src/base/xml.h index 57c0227..5ea3b64 100644 --- a/src/base/xml.h +++ b/src/base/xml.h @@ -442,6 +442,7 @@ class XMLElement XMLContent& AddContent(const XMLContent&); #else XMLContent** GetContents(); + //unsigned int GetContents(XMLContent**); int AddContent(XMLContent* v,int InsertBeforeElement); int AddContent(const char*,int); #endif @@ -603,6 +604,7 @@ class XMLVariable void SetValueUInt64(unsigned long long); void SetValueFloat(float); void SetFormattedValue(const char* fmt,...); + template T GetFormattedValue(const char* fmt) { size_t p = GetValue(0); @@ -612,6 +614,8 @@ class XMLVariable sscanf(d,fmt,&x); return x; } + + template void SetValueX(T t,const char* fmt); template T GetValueX(const char* fmt); XMLVariable* Duplicate(); @@ -689,6 +693,11 @@ class XMLContent operator const char*(); size_t GetValue(char*,int NoDecode = 0) const; void SetValue(const char*,int NoDecode = 0); +///////////////////////////////////////////////// PROYECTO FRAN /////////////////////// + //int GetValueCad (char* cad); + size_t GetSize (); + +///////////////////////////////////////////////////////////////////////////////////// size_t MemoryUsage(); void CompressMemory(); bool IntegrityTest(); diff --git a/src/base/xmlaux.cpp b/src/base/xmlaux.cpp new file mode 100644 index 0000000..44e5420 --- /dev/null +++ b/src/base/xmlaux.cpp @@ -0,0 +1,334 @@ +/********************************************************************** + * + * This code is part of the MRcore project + * Author: -----------anyone + * + * MRcore is licenced under the Common Creative License, + * Attribution-NonCommercial-ShareAlike 3.0 + * + * You are free: + * - to Share - to copy, distribute and transmit the work + * - to Remix - to adapt the work + * + * Under the following conditions: + * - Attribution. You must attribute the work in the manner specified + * by the author or licensor (but not in any way that suggests that + * they endorse you or your use of the work). + * - Noncommercial. You may not use this work for commercial purposes. + * - Share Alike. If you alter, transform, or build upon this work, + * you may distribute the resulting work only under the same or + * similar license to this one. + * + * Any of the above conditions can be waived if you get permission + * from the copyright holder. Nothing in this license impairs or + * restricts the author's moral rights. + * + * It is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. + **********************************************************************/ +#include "xmlaux.h" + + +namespace mr +{ + +string XMLAux::string_ConvertBool (bool b) + { + string cad; + if (b) cad="true"; + else cad="false"; + return cad; + } + +double XMLAux::GetValueDouble(XMLVariable* v) + { + if (!v) + return 0.00; + char cad[50]={0}; + v->GetValue(cad); + istringstream istr(cad); + double p=0.000; + istr>>p; + return p; + } +string XMLAux::GetValueCadena (XMLVariable* v) +{ + if (!v) + return "noName"; + char t[50] = {0}; + v->GetValue(t); + string cad=string(t); + return cad; +} +string XMLAux::GetNameElement (XMLElement *elem) +{ + if (!elem) + return "error"; + char t[50] = {0}; + elem->GetElementName(t); + string cad=string(t); + return cad; +} + + +bool XMLAux::matrixTypeEuler (string cad) +{ + int beg=0,end=0; + for(int i=0;iGetChildrenNum(); +// string className = obj->getClassName(); +// if (num) +// { +// XMLElement** childs=parent->GetChildren(); +// for (int i=0;iFindVariableZ("id")->GetValueInt()==index_obj) +// return childs[i]; +// } +// } +// return 0; +//} + +void XMLAux::GetValueOwnerAndTcp(XMLVariable* v, int* ownerAndTcp) + { + char cad[50]={0}; + v->GetValue(cad); + string check=string(cad); + stringstream str,str1; + bool toTcp=false; + for (int i=0;i<(int)check.size();i++) + { + if (check[i]==',' || check[i]=='.') + { + toTcp=true; + i++; + } + if (!toTcp) + str<>(int)ownerAndTcp[0]; + str1>>(int)ownerAndTcp[1]; + + } +string XMLAux::GetNameLinkTo (XMLVariable* v) +{ + char cad[50]={0}; + v->GetValue(cad); + string check=string(cad); + int ident=0; + stringstream str; + + for (int i=0;i<(int)check.size();i++) + { + if (check[i]=='$') + { + i++; + ident++; + } + if(ident<2) + str< XMLAux::GetNameOwnerAndTcp(XMLVariable* v) +{ + vector ownerAndTcp; + string owner=string(); + string tcp=string(); + ownerAndTcp.push_back(owner); + ownerAndTcp.push_back(tcp); + + char cad[50]={0}; + v->GetValue(cad); + string check=string(cad); + + bool toTcp=false; + int j=0; + stringstream str1,str2; + + for (int i=0;i<(int)check.size();i++) + { + if (check[i]=='$') + i++; + + if (check[i]==',' || check[i]=='.') + { + toTcp=true; + j=0; + i++; + } + if (!toTcp) + { + str1<>(int)ind; + return ind; +} + + +string XMLAux::getTypeConectionLink (XMLVariable* v,bool linktotcp) +{ + char t[50] = {0}; + v->GetValue(t); + string cad=string(t); + int ident=0,separate=0; + + for (int i=0;i<(int)cad.size();i++) + { + if (cad[i]=='$') + ident++; + if (cad[i]==',' || cad[i]=='.') + separate++; + } + + if (ident==2 && separate==1 && linktotcp) + return "conectionNames"; + + else if (ident==0 && separate==1 && linktotcp) + return "conectionId"; + + else if (ident==2 && !separate && !linktotcp) + return "conectionNames"; + + else if (ident==0 && !separate && !linktotcp) + return "conectionId"; + + else + return "error"; + +} + +string XMLAux::getOnlyNameTcp(string name) +{ + stringstream str; + + for (int i=0;i<(int)name.size();i++) + { + if (name[i]=='[') + break; + str< +#include +#include +#include "xml.h" +#include +//#include "object.h" + +using namespace std; + + +namespace mr +{ + + + //XML &operator<<(XML &xml, const Object &obj); + + +class XMLAux +{ +public: + +XMLAux(void){}; + +template static string string_Convert (T x) +{ + stringstream str; + string cad; + str< GetNameOwnerAndTcp(XMLVariable* v); +static string getTypeConectionLink (XMLVariable* v, bool linktotcp=true); +static int getIndTcp (string cad); +static string setLinkToTcpDefault (string name_owner,int index_tcp,int id_owner); +static string setLinkToTcp (string name_owner,string name_tcp,int index_tcp,int id_owner); +static string getOnlyNameTcp(string name); + +}; + + + +}; +#endif //__MRCORE__XMLAUX__H diff --git a/src/base/xmlfile.cpp b/src/base/xmlfile.cpp index c439d94..54784f1 100644 --- a/src/base/xmlfile.cpp +++ b/src/base/xmlfile.cpp @@ -34,47 +34,227 @@ namespace mr { +XMLfile::XMLfile (const char *file) +{ + xml = new XML(file); + root = xml->GetRootElement(); + + checkOverwrite=false; + + if (root->GetChildrenNum()) + checkOverwrite=true; + +} -Object *XMLfile::load(const char *file) //loads, parses the file and creates the object +XMLfile::XMLfile (XMLElement *parent) { -return 0; + root = parent; } -bool XMLfile::save(Object* pObj,const char *file) //saves the XML file created with the object + +XMLfile::~XMLfile() { - return false; +// delete xml; +} + + +XMLfile& XMLfile::operator<<(Object *obj) +{ + write(obj); + return (*this); +} + +XMLfile& XMLfile::operator<<(int _id) +{ +// XMLAux aux; + XMLVariable* ident= new XMLVariable ("id",XMLAux::string_Convert(_id).c_str()); + pElem[_id]->AddVariable(ident); + return (*this); + +} + -}/* -void Stream::write(Object* pObj) + +void XMLfile::write (Object* pObj) { - //id - //size - // First, the "classname". - string className = pObj->getClassName(); - (*this)<RemoveAllElements();//remove old xml elements + checkOverwrite=false; + } + + string className = pObj->getClassName(); + XMLElement* obj_xml=new XMLElement (root,className.c_str()); + + pObj->writeToXML(obj_xml); - // Next, the object data. - pObj->writeToStream(*this); + root->AddElement(obj_xml); + pElem.push_back(obj_xml);//save every XMLElements_Objects in pElem + } -Object* Stream::read() +Object* XMLfile::read (XMLElement* obj) { - //id - //size - string className; - (*this)>>className; - Object* pObj=Object::create(className); + char className[50]={0}; + obj->GetElementName(className); + Object* pObj=Object::create(string(className)); if(pObj) - pObj->readFromStream(*this); - if(!good()) { - delete pObj; + pObj->readFromXML(obj); + return pObj; + } + else return 0; +} + +bool XMLfile::save() +{ + xml->Save(); + return true; +} + + +bool XMLfile::save(Object* pObj,const char *file) //saves the XML file created with the object +{ + // Create the XML FILE or if it already exists, it will be overwritten + + XML* xml = new XML(file); + XMLElement* root = xml->GetRootElement(); + + string className = pObj->getClassName(); + XMLElement* obj_xml=new XMLElement (root,className.c_str()); + + pObj->writeToXML(obj_xml);//execute the writing process XML + pElem.push_back(obj_xml);//save every XMLElements_Objects in pElem + + root->AddElement(obj_xml); + xml->Save(); + + return true; + +} + + +Object* XMLfile::load(const char *file) //loads, parses the file and creates the object +{ + + //import another objects saved in another files xml + if (importFromXML()) + { + if (root->FindElementZ("World")) + root->FindElementZ("World")->AddElement(imported); + else + root->AddElement(imported); } - return pObj; + + XMLElement** objects=root->GetChildren(); + int num=root->GetChildrenNum(); + + char className[50]={0}; + for (int i=0; iFindElementZ("importfrom")) + { + objects[i]->GetElementName(className); + Object* pObj=Object::create(string(className)); + if(pObj) + { + pObj->readFromXML(objects[i]); + return pObj; + } + } + } + + return 0; } -*/ +bool XMLfile::importFromXML () +{ + + //Ejemplo: + string name; + if (root->FindElementZ("importfrom")) + { + XMLElement* import=root->FindElementZ("importfrom"); + if (import->FindVariableZ("xml")) + { + name=XMLAux::GetValueCadena(import->FindVariableZ("xml")); + XML* xmlImport = new XML(name.c_str()); + XMLElement* rootImport = xmlImport->GetRootElement(); + + + if (import->FindVariableZ("element")) + { + name=string(); + name=XMLAux::GetValueCadena(import->FindVariableZ("element")); + + if (rootImport->FindElementZ(name.c_str())) + { + imported=rootImport->FindElementZ(name.c_str())->Duplicate(); + if (import->FindVariableZ("name")) + { + name=string(); + name=XMLAux::GetValueCadena(import->FindVariableZ("name")); + + if (imported->FindVariableZ("name")) + imported->FindVariableZ("name")->SetValue(name.c_str()); + else + { + XMLVariable* _name=new XMLVariable ("name",name.c_str()); + imported->AddVariable(_name); + } + } + return true; + } + + else //search the XMLElement in another deep just in case + { + int num=rootImport->GetAllChildrenNum(); + XMLElement** childsImport=new XMLElement*[num]; + rootImport->GetAllChildren(childsImport); + for (int i=0;iFindElementZ(name.c_str())) + { + imported=childsImport[i]->FindElementZ(name.c_str())->Duplicate(); + + if (import->FindVariableZ("name")) + { + name=string(); + name=XMLAux::GetValueCadena(import->FindVariableZ("name")); + + if (imported->FindVariableZ("name")) + imported->FindVariableZ("name")->SetValue(name.c_str()); + else + { + XMLVariable* _name=new XMLVariable ("name",name.c_str()); + imported->AddVariable(_name); + } + } + return true; + } + } + + + } + + + } + + } + + + } + + return false; +} + +/*overwrite() +{ + + + +} +*/ } ;//mr diff --git a/src/base/xmlfile.h b/src/base/xmlfile.h index 4269761..9ee7572 100644 --- a/src/base/xmlfile.h +++ b/src/base/xmlfile.h @@ -31,8 +31,10 @@ #ifndef __MRCORE__XMLFILE__H #define __MRCORE__XMLFILE__H -#include "xml.h" +#include "xmlaux.h" +#include +using namespace std; //using namespace std; @@ -41,6 +43,7 @@ namespace mr { class Object; +class XMLElement; /*! \class XMLfile \brief open, read, and writes a XMLfile for one object. @@ -49,13 +52,35 @@ class Object; class XMLfile { public: - static Object *load(const char *file); //loads and parses the file - static bool save(Object* pObj,const char *file); //saves the XML file created with the object -}; //End of class Stream + //constructor and destructor + //this class cannot be directly instantiated + XMLfile(const char *file="xml_file.xml"); + XMLfile (XMLElement *parent); + ~XMLfile(); + Object * load(const char *file="xml_file.xml"); //loads and parses the file + bool save(Object* pObj,const char *file="xml_file.xml"); //saves the XML file created with the object + bool save(); + void write (Object* pObj); + Object* read (XMLElement* obj); + bool importFromXML (); + + XMLfile& operator<<(Object *obj); + XMLfile& operator<<(int _id); + + vector getXMLElementsObjects () {return pElem;} +private: + bool checkOverwrite; + vector pElem; + XML* xml; + XMLElement* root; + XMLElement* imported; + + +}; //End of class XMLFile }; #endif //__MRCORE__XMLfile_H diff --git a/src/data/data.cpp b/src/data/data.cpp index 16ad53a..e00d132 100644 --- a/src/data/data.cpp +++ b/src/data/data.cpp @@ -41,5 +41,30 @@ void Data::readFromStream(Stream& stream) stream>>timestamp.seconds>>timestamp.microseconds; } +void Data::writeToXML(XMLElement* parent) +{ + //XMLElement* dat=new XMLElement(parent,"data"); + XMLVariable* sec= new XMLVariable ("seconds",XMLAux::string_Convert(timestamp.seconds).c_str()); + XMLVariable* microsec= new XMLVariable ("microseconds",XMLAux::string_Convert(timestamp.microseconds).c_str()); + + parent->AddVariable(sec); + parent->AddVariable(microsec); + //parent->AddElement(dat); +} + +void Data::readFromXML(XMLElement* parent) +{ + if (parent->FindVariableZ("seconds")) + { + XMLVariable* sec=parent->FindVariableZ("seconds"); + timestamp.seconds=sec->GetValueInt(); + } + if (parent->FindVariableZ("microseconds")) + { + XMLVariable* microsec=parent->FindVariableZ("microseconds"); + timestamp.microseconds=microsec->GetValueInt(); + } +} + } //mr \ No newline at end of file diff --git a/src/data/data.h b/src/data/data.h index e0be692..c80cdef 100644 --- a/src/data/data.h +++ b/src/data/data.h @@ -33,6 +33,7 @@ #include "base/globject.h" #include "system/time.h" +#include "../base/xmlaux.h" namespace mr { @@ -45,6 +46,8 @@ class Data: public GLObject public: void writeToStream(Stream& stream); void readFromStream(Stream& stream); + void writeToXML(XMLElement* parent); + void readFromXML(XMLElement* parent); //annotates this data item with the current time void timeStamp() { diff --git a/src/data/odometry.cpp b/src/data/odometry.cpp index f9ba370..a752c22 100644 --- a/src/data/odometry.cpp +++ b/src/data/odometry.cpp @@ -60,6 +60,24 @@ void Odometry::readFromStream(Stream& stream) pose.readFromStream(stream); } +void Odometry::writeToXML(XMLElement* parent) +{ + XMLElement* odo=new XMLElement(parent,"odometry"); + Data::writeToXML(odo); + pose.writeToXML(odo); + parent->AddElement(odo); +} + +void Odometry::readFromXML(XMLElement* parent) +{ + if (parent->FindElementZ("odometry")) + { + XMLElement* odo=parent->FindElementZ("odometry"); + Data::readFromXML(odo); + pose.readFromXML(odo); + } +} + void Odometry::drawGL() { diff --git a/src/data/odometry.h b/src/data/odometry.h index d51ebe3..2bfbea6 100644 --- a/src/data/odometry.h +++ b/src/data/odometry.h @@ -58,6 +58,9 @@ class Odometry :public Data //Serializers virtual void writeToStream(Stream& stream); virtual void readFromStream(Stream& stream); + virtual void writeToXML(XMLElement* parent); + virtual void readFromXML(XMLElement* parent); + virtual void drawGL();//>maxVertex.x>>maxVertex.y>>maxVertex.z; stream>>minVertex.x>>minVertex.y>>minVertex.z; } +void BoundingBox::writeToXML(XMLElement* parent) +{ +/* XMLElement* bound=new XMLElement(parent,"boundingbox"); + XMLElement* maxver=new XMLElement(parent,"maxVertex"); + XMLElement* minver=new XMLElement(parent,"minVertex"); + + XMLVariable* xmax= new XMLVariable("x",(char*)cadena(maxVertex.x).c_str()); + XMLVariable* ymax=new XMLVariable("y",(char*)cadena(maxVertex.y).c_str()); + XMLVariable* zmax=new XMLVariable("z",(char*)cadena(maxVertex.z).c_str()); + XMLVariable* xmin= new XMLVariable("x",(char*)cadena(minVertex.x).c_str()); + XMLVariable* ymin=new XMLVariable("y",(char*)cadena(minVertex.y).c_str()); + XMLVariable* zmin=new XMLVariable("z",(char*)cadena(minVertex.z).c_str()); + + parent->AddElement(bound); + bound->AddElement(maxver); + bound->AddElement(minver); + + maxver->AddVariable(xmax); + maxver->AddVariable(ymax); + maxver->AddVariable(zmax); + + minver->AddVariable(xmin); + minver->AddVariable(ymin); + minver->AddVariable(zmin);*/ + +} +void BoundingBox::readFromXML(XMLElement* parent) +{ + +} +//// BoundingBox operator*(Transformation3D T, BoundingBox& b) { BoundingBox ret; diff --git a/src/math/boundingbox.h b/src/math/boundingbox.h index 4205d8a..1b1ca58 100644 --- a/src/math/boundingbox.h +++ b/src/math/boundingbox.h @@ -36,6 +36,7 @@ #include "transformation3d.h" #include "segment3d.h" #include "face.h" +#include "../base/xmlfile.h" namespace mr { /*! @@ -62,8 +63,12 @@ class BoundingBox: public Object BoundingBox& operator = (const BoundingBox& p){minVertex=p.minVertex;maxVertex=p.maxVertex;validData=p.validData;return *this;} //copy constructor BoundingBox(const BoundingBox &p){(*this)=p;} +//serialization virtual void writeToStream(Stream& stream); virtual void readFromStream(Stream& stream); + virtual void writeToXML(XMLElement* parent); + virtual void readFromXML(XMLElement* parent); + virtual void drawGL(); //methods void reset(){validData=false;} diff --git a/src/math/face.cpp b/src/math/face.cpp index dde2370..93bcd52 100644 --- a/src/math/face.cpp +++ b/src/math/face.cpp @@ -126,6 +126,113 @@ void Face::readFromStream(Stream& stream) stream>>r>>g>>b>>alfa; //add vertex updates the remainer internal data } +void Face::writeToXML(XMLElement* parent)//,int numface) +{ + int num=(int)(vertex.size()); + + XMLElement* fac=new XMLElement(parent,"face"); + XMLElement* vert=new XMLElement(fac,"vertex"); + XMLContent* c; + + for(int i=0;iAddContent(c,0); + + } + + + bool defaultR=true, defaultG=true, defaultB=true; + + XMLElement* colour=new XMLElement(fac,"colour"); + + if (r!=1.0) + { + XMLVariable* re= new XMLVariable("r",XMLAux::string_Convert(r).c_str()); + colour->AddVariable(re); + defaultR=false; + } + if (g!=1.0) + { + XMLVariable* gr= new XMLVariable("g",XMLAux::string_Convert(g).c_str()); + colour->AddVariable(gr); + defaultG=false; + } + if (b!=1.0) + { + XMLVariable* bl= new XMLVariable("b",XMLAux::string_Convert(b).c_str()); + colour->AddVariable(bl); + defaultB=false; + } + + if (!defaultR || !defaultG || !defaultB) + fac->AddElement(colour); + + + parent->AddElement(fac); + fac->AddElement(vert); + base.writeToXML(fac); + +} + +void Face::readFromXML(XMLElement* parent) +{ + + XMLElement* fac; + char check_parent[50]={0}; + parent->GetParent()->GetElementName(check_parent); + + if(parent->FindElementZ("face")) + fac=parent->FindElementZ("face"); + + else if(string(check_parent)=="FaceSetPart") + fac=parent; + + if (fac->FindElementZ("vertex")) + { + + + int num=fac->FindElementZ("vertex")->GetContentsNum(); + + if(num) + { + + XMLContent** c=fac->FindElementZ("vertex")->GetContents(); + string cad;//,type="vertex"; + for (int i=0;iGetSize()); + c[i]->GetValue((char*)cad.c_str()); + vector aux_vec=Vector3D::stringToVectorVector3D(cad); + for (int j=0;jFindElementZ("colour")) + { + XMLElement* colour=fac->FindElementZ("colour"); + + if(colour->FindVariableZ("r")) + r=colour->FindVariableZ("r")->GetValueFloat(); + if(colour->FindVariableZ("g")) + g=colour->FindVariableZ("g")->GetValueFloat(); + if(colour->FindVariableZ("b")) + b=colour->FindVariableZ("b")->GetValueFloat(); + } + + base.readFromXML(fac); +} + void Face::updateData() { int sign,signini,i; diff --git a/src/math/face.h b/src/math/face.h index 62b9518..69ae2b9 100644 --- a/src/math/face.h +++ b/src/math/face.h @@ -79,8 +79,12 @@ class Face: public GLObject //copy constructor Face(const Face &f){(*this)=f;} void setColor(float _r, float _g,float _b,float _alfa){r=_r;g=_g;b=_b;alfa=_alfa;} +//serialization virtual void writeToStream(Stream& stream); virtual void readFromStream(Stream& stream); + virtual void writeToXML(XMLElement* parent);//,int numface=0); + virtual void readFromXML(XMLElement* parent); + virtual void drawGL(); void drawPrimitive(bool dir=true); void setBase(const Transformation3D &inbase); diff --git a/src/math/matrix3x3.h b/src/math/matrix3x3.h index fd9ffd0..7752a79 100644 --- a/src/math/matrix3x3.h +++ b/src/math/matrix3x3.h @@ -77,7 +77,7 @@ class Matrix3x3{ inline Matrix3x3 & operator*= (double v); inline Matrix3x3 & operator/= (double v); inline Vector3D operator * (const Vector3D &v) const; - + inline bool operator== (const Matrix3x3 &m ); //methods inline double getDeterminant() const; inline Matrix3x3 inverted() const; @@ -93,6 +93,18 @@ class Matrix3x3{ }; //Implementation of inline methods///////////////////////////////////// + +bool Matrix3x3::operator== (const Matrix3x3 &m ) +{ + if (mat[0][0]==m.mat[0][0] && mat[0][1]==m.mat[0][1] && mat[0][2]==m.mat[0][2] && + mat[1][0]==m.mat[1][0] && mat[1][1]==m.mat[1][1] && mat[1][2]==m.mat[1][2] && + mat[2][0]==m.mat[2][0] && mat[2][1]==m.mat[2][1] && mat[2][2]==m.mat[2][2]) + return true; + else + return false; + +} + Matrix3x3::Matrix3x3(double diag) { mat[0][0]=mat[1][1]=mat[2][2]=diag; @@ -218,6 +230,5 @@ Vector3D Matrix3x3::operator * (const Vector3D &v) const { return Vector3D( v*(mat[0]),v*(mat[1]),v*(mat[2])); } - } #endif //__MRCORE__MATRIX3x3_H diff --git a/src/math/transformation3d.cpp b/src/math/transformation3d.cpp index 992a1ca..3f77920 100644 --- a/src/math/transformation3d.cpp +++ b/src/math/transformation3d.cpp @@ -39,7 +39,7 @@ ostream& operator<<(ostream& os, const Transformation3D& p) os<>m[2][0]>>m[2][1]>>m[2][2]; stream>>position[0]>>position[1]>>position[2]; } +void Transformation3D::writeToXML(XMLElement* parent) +{ + //Matrix3x3 defaultM(1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0); + Vector3D defaultPos(0.0,0.0,0.0); + bool dfPos=false,dfOrient=false; + Vector3D defaultOrient(0.0,0.0,0.0); + Vector3D orientObj; + orientation.getRPY(orientObj.x,orientObj.y,orientObj.z); + + if (position==defaultPos)//default value + dfPos=true; + if (defaultOrient==orientObj) + dfOrient=true; + + if (!dfOrient) + { + XMLElement* orient=new XMLElement(parent,"orientation"); + XMLContent* mat; + + + //for(int i=0;i<3;i++) + //{ + //write Euler form in XML + mat=new XMLContent(orient,-1,Vector3D :: vector3DToString(orientObj).c_str()); + orient->AddContent(mat,0); + //} + + parent->AddElement(orient); + } + + if (!dfPos) + { + XMLContent* vec_posi; + XMLElement* posi= new XMLElement(parent,"position"); + vec_posi=new XMLContent(parent,-1,Vector3D :: vector3DToString(position).c_str()); + + posi->AddContent(vec_posi,0); + parent->AddElement(posi); + } +} + +void Transformation3D::readFromXML(XMLElement* parent) +{ + string cad; + Matrix3x3 &m=orientation; + XMLElement* orient; + + if(parent->FindElementZ("orientation")) + { + orient=parent->FindElementZ("orientation"); + + int num=orient->GetContentsNum(); + if(num) + { + + XMLContent** c=orient->GetContents(); + + for (int i=0;iGetSize()); + c[i]->GetValue((char*)cad.c_str()); + + if (XMLAux::matrixTypeEuler(cad)) //check if the orientation is Euler form + { + Vector3D p; + p=Vector3D::stringToVector3D(cad); + orientation=OrientationMatrix(p.x,p.y,p.z); + } + else // it's matrix form + { + vector aux_vec=Vector3D::stringToVectorVector3D(cad); + m[0][0]=aux_vec[0].x;m[0][1]=aux_vec[0].y;m[0][2]=aux_vec[0].z; + m[1][0]=aux_vec[1].x;m[1][1]=aux_vec[1].y;m[1][2]=aux_vec[1].z; + m[2][0]=aux_vec[2].x;m[2][1]=aux_vec[2].y;m[2][2]=aux_vec[2].z; + } + } + + + } + } + + + XMLElement* posi; + if(parent->FindElementZ("position")) + { + + posi=parent->FindElementZ("position"); + int num=posi->GetContentsNum(); + + if(num) + { + cad=string(); + XMLContent** c=posi->GetContents(); + cad.resize(c[0]->GetSize()); + c[0]->GetValue((char*)cad.c_str()); + position=Vector3D::stringToVector3D(cad); + } + + } + + +} + + void Transformation3D::transformGL() { diff --git a/src/math/transformation3d.h b/src/math/transformation3d.h index 00ddca0..ade8f69 100644 --- a/src/math/transformation3d.h +++ b/src/math/transformation3d.h @@ -35,6 +35,8 @@ #include "vector3d.h" #include "orientationmatrix.h" #include "../base/globject.h" +#include "../base/xmlaux.h" +#include "../base/xml.h" using namespace std; namespace mr @@ -111,9 +113,11 @@ class Transformation3D{ Transformation3D inverted() const; void transformGL(); void drawGL(); - +//serialization void writeToStream(Stream& stream); void readFromStream(Stream& stream); + void writeToXML(XMLElement* parent); + void readFromXML(XMLElement* parent); //Metric based module taking into account angles weighted by L2 double module() { diff --git a/src/math/trianglemesh.cpp b/src/math/trianglemesh.cpp index 14a2275..2ad5559 100644 --- a/src/math/trianglemesh.cpp +++ b/src/math/trianglemesh.cpp @@ -31,7 +31,9 @@ #include "trianglemesh.h" #include "mrmath.h" #include +#include +using namespace std; namespace mr{ IMPLEMENT_MR_OBJECT(TriangleMesh) @@ -102,6 +104,192 @@ void TriangleMesh::readFromStream(Stream& stream) } box.readFromStream(stream); } + +void TriangleMesh::writeToXML(XMLElement* parent) +{ + int i,num=(int)vertex.size(); + + //XMLElement* tria=new XMLElement(parent,"triangleMesh"); + + + XMLElement* vert=new XMLElement(parent,"vertex"); + XMLContent* c; + + for(i=0;iAddContent(c,0); + + } + + num=(int)edges.size(); + XMLElement* vert_edge=new XMLElement(parent,"TMEdge"); + + for(i=0;iAddContent(c,0); + + } + + num=(int)triangles.size(); + XMLElement* vert_triangles=new XMLElement(parent,"TMTriangle"); + + XMLElement* norm=new XMLElement(vert_triangles,"vertex_normal"); + XMLElement* fac=new XMLElement(vert_triangles,"vertex_face"); + XMLElement* edg=new XMLElement(vert_triangles,"vertex_edges"); + for(i=0;iAddContent(c,0); + + c=new XMLContent(fac,-1,Vector3D :: vector3DToString(_faces).c_str()); + fac->AddContent(c,0); + + c=new XMLContent(edg,-1,Vector3D :: vector3DToString(_edges).c_str()); + edg->AddContent(c,0); + + } + vert_triangles->AddElement(norm); + vert_triangles->AddElement(fac); + vert_triangles->AddElement(edg); + + //parent->AddElement(tria); + parent->AddElement(vert); + parent->AddElement(vert_edge); + parent->AddElement(vert_triangles); + + //box.writeToXML(tria); + +} +void TriangleMesh::readFromXML(XMLElement* parent) +{ + string cad; + + if (parent->FindElementZ("vertex")) + { + XMLElement* vert=parent->FindElementZ("vertex"); + + + int num=vert->GetContentsNum(); + if(num) + { + + XMLContent** c=vert->GetContents(); + + for (int i=0;iGetSize()); + c[i]->GetValue((char*)cad.c_str()); + vector aux_vec=Vector3D::stringToVectorVector3D(cad); + vertex=aux_vec; + } + } + } + + if (parent->FindElementZ("TMEdge")) + { + XMLElement* vert_edge=parent->FindElementZ("TMEdge"); + + + int num=vert_edge->GetContentsNum(); + if(num) + { + + XMLContent** c=vert_edge->GetContents(); + + for (int i=0;iGetSize()); + c[i]->GetValue((char*)cad.c_str()); + vector aux_vec=Vector3D::stringToVectorVector3D(cad); + for (int j=0;j<(int)aux_vec.size();j++) + { + eaux.a=(int)aux_vec[j].x; + eaux.b=(int)aux_vec[j].y; + edges.push_back(eaux); + } + } + } + } + + + if (parent->FindElementZ("TMTriangle")) + { + XMLElement* vert_triangles=parent->FindElementZ("TMTriangle"); + + if (vert_triangles->FindElementZ("vertex_normal") && vert_triangles->FindElementZ("vertex_face") && vert_triangles->FindElementZ("vertex_edges")) + { + XMLElement* norm=vert_triangles->FindElementZ("vertex_normal"); + XMLElement* fac=vert_triangles->FindElementZ("vertex_face"); + XMLElement* edg=vert_triangles->FindElementZ("vertex_edges"); + + TMTriangle taux; + + int num=norm->GetContentsNum(); + int num2=fac->GetContentsNum(); + int num3=edg->GetContentsNum(); + + if(num && num2 && num3) + { + + XMLContent** c=norm->GetContents(); + XMLContent** c2=fac->GetContents(); + XMLContent** c3=edg->GetContents(); + + for (int i=0;iGetSize()); + c[i]->GetValue((char*)cad.c_str()); + vector aux_vec=Vector3D::stringToVectorVector3D(cad); + + cad=string(); + cad.resize(c2[i]->GetSize()); + c2[i]->GetValue((char*)cad.c_str()); + vector aux_vec2=Vector3D::stringToVectorVector3D(cad); + + cad=string(); + cad.resize(c3[i]->GetSize()); + c3[i]->GetValue((char*)cad.c_str()); + vector aux_vec3=Vector3D::stringToVectorVector3D(cad); + + if (aux_vec.size()==aux_vec2.size() && aux_vec2.size()==aux_vec3.size()) + { + + for (int j=0;j<(int)aux_vec.size();j++) + { + taux.normal.x=aux_vec[j].x; + taux.normal.y=aux_vec[j].y; + taux.normal.z=aux_vec[j].z; + + taux.a=(int)aux_vec2[j].x; + taux.b=(int)aux_vec2[j].y; + taux.c=(int)aux_vec2[j].z; + + taux.ea=(int)aux_vec3[j].x; + taux.eb=(int)aux_vec3[j].y; + taux.ec=(int)aux_vec3[j].z; + + triangles.push_back(taux); + } + } + } + } + } + } +} + + + int TriangleMesh::addVertex(const Vector3D &v) { int i; diff --git a/src/math/trianglemesh.h b/src/math/trianglemesh.h index 0cfdca5..7109c7d 100644 --- a/src/math/trianglemesh.h +++ b/src/math/trianglemesh.h @@ -94,8 +94,12 @@ class TriangleMesh: public GLObject triangles=f.triangles;box=f.box;return *this;listGL=0; generateGLlist=true;} //copy constructor TriangleMesh(const TriangleMesh &f){(*this)=f;} +//serialization virtual void writeToStream(Stream& stream); virtual void readFromStream(Stream& stream); + virtual void writeToXML(XMLElement* parent); + virtual void readFromXML(XMLElement* parent); + virtual void drawGL(); bool addTriangle(Vector3D a,Vector3D b, Vector3D c);//normal vector is computed throug the cross product cw //special operator that obtains the TriangleMesh transformed by the T reference system diff --git a/src/math/vector3d.cpp b/src/math/vector3d.cpp index 3cb3e2e..8260b5d 100644 --- a/src/math/vector3d.cpp +++ b/src/math/vector3d.cpp @@ -57,4 +57,157 @@ Vector3D Vector3D::getUnitaryVector() const Vector3D ret(*this); return ret.normalize(); } + +////////////////////////////////////////////////////////// +Vector3D Vector3D::stringToVector3D (string cad) +{ + Vector3D p(0.00); + + if (cad.empty()) + return p; + if (cad[0]!='{') + return p; + if (cad[cad.size()-1]!='}') + return p; + + int beg=0,end=0,com=0; + for(int i=0;i>p[coma++]; + aux.erase(0,j); + j=0; + + } + else if (cad[i]=='}') + { + istringstream istr(aux); + istr>>p[coma++]; + return p; + } + else if(cad [i]==' ') + continue; + + else + { + aux[j]=cad[i]; + j++; + } + + } + return p; + +} + +vector Vector3D::stringToVectorVector3D (string cad)//, string type_id) +{ + vector p,aux; + p.push_back(Vector3D(1.00,0.00,0.00));//default matrix orientation + p.push_back(Vector3D(0.00,1.00,0.00)); + p.push_back(Vector3D(0.00,0.00,1.00)); + + if (cad.empty()) + return p; + if (cad[0]!='{') + return p; + if (cad[cad.size()-1]!='}') + return p; + + int beg=0,end=0,count=0,ind=0; + //vector s; + vector size; + for (int i=0;ibeg) + return p; + + } + + if (beg!=end) + return p; + + string s; + int j=0,copy=0; + ind=0; + + for (int i=0;i +#include #include #include #include "base/stream.h" - +#include using namespace std; namespace mr @@ -120,6 +121,11 @@ class Vector3D{ Vector3D getUnitaryVector() const; +/////////////////////////////////////////////////////////////////// +static Vector3D stringToVector3D (string cad); +static vector stringToVectorVector3D (string cad); +static string vector3DToString (Vector3D p); +///////////////////////////////////////////////////////////////// //Punto3D is a typedef of Vector3D inline double distanceTo(const Vector3D& p) const { return ((*this) - p).module(); } diff --git a/src/simulation/EuitiBotSim.cpp b/src/simulation/EuitiBotSim.cpp new file mode 100644 index 0000000..ff8dc6e --- /dev/null +++ b/src/simulation/EuitiBotSim.cpp @@ -0,0 +1,822 @@ +#include "EuitiBotSim.h" +#include + + + +namespace mr +{ + IMPLEMENT_MR_OBJECT(EuitiBotSim) + + void EuitiBotSim::writeToStream(Stream& stream) + {SolidEntity::writeToStream(stream);} + void EuitiBotSim::readFromStream(Stream& stream) + {SolidEntity::readFromStream(stream);} + void EuitiBotSim::writeToXML(XMLElement* parent){ + SolidEntity::writeToXML(parent); + } + void EuitiBotSim::readFromXML(XMLElement* parent){ + SolidEntity::readFromXML(parent); + } + + EuitiBotSim::EuitiBotSim(void) + { + name = "EuitiBot"; + for(int j=0;j<6;j++)q_init.push_back(0); + + vector list; + + //// +++++++++++++++++++++++++++++ //// + //// +++++++++++++++++++++++++++++ //// + //// COMENZAMOS A DIBUJAR EL ROBOT //// + //// +++++++++++++++++++++++++++++ //// + //// +++++++++++++++++++++++++++++ //// + + +/////////////////////////////////////////////////////////////////// +////////////////////////// BASE = LINK[0]////////////////////////// +/////////////////////////////////////////////////////////////////// + + //NUEVO GRUPO DE PIEZAS + ComposedEntity *link=new ComposedEntity; + link->setName("Base"); + +//**************************************// +// LA BASE ESTA FORMADA POR UN CILINDRO // +//**************************************// + + // CILINDRO(Cilindro) + CylindricalPart *auxCyl=new CylindricalPart(0.5,0.425);//Altura y radio + auxCyl->setColor(255,233,0); + (*link)+=auxCyl; + + // AÑADIMOS AL VECTOR LINKS + (*this)+=link; + links.push_back(link); + + + +//////////////////////////////////////////////////////////////////////////// +//////////////////////////ARTICULACION 1 = LINK[1]////////////////////////// +//////////////////////////////////////////////////////////////////////////// + + //JOINT[0] = ARTICULACION 1 ROTACIONAL + SimpleJoint *auxJoint=new SimpleJoint(PI/2,-PI/2,true,0,Z_AXIS,false); + auxJoint->setRelativePosition(Vector3D(0,0,0.52)); + auxJoint->LinkTo(links[0]); + auxJoint->setValue(q_init[0]); + joints.push_back(auxJoint); + + //NUEVO GRUPO DE PIEZAS + link=new ComposedEntity; + link->setName("Link 1"); + +//***********************************************************************// +// LA ARTICULACION 1 ESTA COMPUESTA POR UN CILINDRO Y DOS PRISMAS IRREG. // +//***********************************************************************// + // CILINDRO(Cilindro) + auxCyl=new CylindricalPart(0.06,0.425);//Altura y radio + auxCyl->setColor(255,233,0); + (*link)+=auxCyl; + + // PRISMA IRREGULAR DERECHO(Pris_Der) + PrismaticPart *auxPrism=new PrismaticPart; + list.push_back(Vector2D(-0.155,0)); + list.push_back(Vector2D(0.155,0)); + list.push_back(Vector2D(0.155,0.5)); + list.push_back(Vector2D(0.125,0.55)); + list.push_back(Vector2D(-0.125,0.55)); + list.push_back(Vector2D(-155,0.5)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(255,233,0); + auxPrism->setHeight(0.1); + auxPrism->setRelativePosition(Vector3D(-0.4,0,0.06)); + auxPrism->setRelativeOrientation(X_AXIS,PI/2); + auxPrism->setRelativeOrientation(Z_AXIS,PI/2); + (*link)+=auxPrism; + + // PRISMA IRREGULAR IZQUIERDO(Cil_Izq) + auxPrism=new PrismaticPart; + list.push_back(Vector2D(-0.155,0)); + list.push_back(Vector2D(0.155,0)); + list.push_back(Vector2D(0.155,0.5)); + list.push_back(Vector2D(0.125,0.55)); + list.push_back(Vector2D(-0.125,0.55)); + list.push_back(Vector2D(-155,0.5)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(255,233,0); + auxPrism->setHeight(0.1); + auxPrism->setRelativePosition(Vector3D(0.3,0,0.06)); + auxPrism->setRelativeOrientation(X_AXIS,PI/2); + auxPrism->setRelativeOrientation(Z_AXIS,PI/2); + (*link)+=auxPrism; + + // SERVO2(Servo2) + auxPrism=new PrismaticPart; + list.push_back(Vector2D(-0.095,-0.2)); + list.push_back(Vector2D(0.095,-0.2)); + list.push_back(Vector2D(0.095,0.2)); + list.push_back(Vector2D(-0.095,0.2)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(0,0,0); + auxPrism->setHeight(0.35); + (*link)+=auxPrism; + + // SERVO3(Servo3) + auxPrism=new PrismaticPart; + list.push_back(Vector2D(-0.095,-0.2)); + list.push_back(Vector2D(0.095,-0.2)); + list.push_back(Vector2D(0.095,0.2)); + list.push_back(Vector2D(-0.095,0.2)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(0,0,0); + auxPrism->setHeight(0.35); + auxPrism->setRelativePosition(Vector3D(-0.29,0,0.3)); + auxPrism->setRelativeOrientation(Z_AXIS,-PI/2); + (*link)+=auxPrism; + + // AÑADIMOS AL VECTOR LINKS + link->LinkTo(joints[0]); + links.push_back(link); + + + + +//////////////////////////////////////////////////////////////////////////// +//////////////////////////ARTICULACION 2 = LINK[2]////////////////////////// +//////////////////////////////////////////////////////////////////////////// + + //JOINT[1] = ARTICULACION 2 ROTACIONAL + auxJoint=new SimpleJoint(25*PI/36 , -25*PI/36,true,0,Z_AXIS,false); + auxJoint->setRelativePosition(Vector3D(0.29,0,1)); + auxJoint->setRelativeOrientation(0,-PI/2,0); + auxJoint->LinkTo(joints[0]); + auxJoint->setValue(q_init[1]); + joints.push_back(auxJoint); + // joints[1]->setDrawReferenceSystem(true); + + //NUEVO GRUPO DE PIEZAS + link=new ComposedEntity; + link->setName("Link 2"); + +//***********************************************************************// +// LA ARTICULACION 2 ESTA COMPUESTA POR 4 CILINDROS Y 6 PRISMAS IRREG. // +//***********************************************************************// + + // CILINDRO IZQUIERDO ABAJO(Cil_Ab_Izq) + auxCyl=new CylindricalPart(0.03,0.175);//Altura y radio + auxCyl->setColor(255,233,0); + auxCyl->setRelativePosition(Vector3D(0,0,0)); + (*link)+=auxCyl; + + // PRISMA IRREGULAR IZQUIERDA(Pris_Izq) + auxPrism=new PrismaticPart; + list.push_back(Vector2D(-0.13,0)); + list.push_back(Vector2D(0.13,0)); + list.push_back(Vector2D(0.13,1.1)); + list.push_back(Vector2D(-0.13,1.1)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(255,233,0); + auxPrism->setHeight(0.03); + auxPrism->setRelativePosition(Vector3D(0,0,0)); + auxPrism->setRelativeOrientation(Z_AXIS,-PI/2); + (*link)+=auxPrism; + + // CILINDRO IZQUIERDO ARRIBA(Cil_Ar_Izq) + auxCyl=new CylindricalPart(0.03,0.175);//Altura y radio + auxCyl->setColor(255,233,0); + auxCyl->setRelativePosition(Vector3D(1.1,0,0)); + (*link)+=auxCyl; + + + // PRISMA IRREGULAR REFUERZO IZQUIERDA(Pris_Ref_Izq) + auxPrism=new PrismaticPart; + list.push_back(Vector2D(-0.4,0)); + list.push_back(Vector2D(0.4,0)); + list.push_back(Vector2D(0.315,0.04)); + list.push_back(Vector2D(-0.315,0.04)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(255,233,0); + auxPrism->setHeight(0.26); + auxPrism->setRelativePosition(Vector3D(0.55,-0.13,0)); + auxPrism->setRelativeOrientation(X_AXIS,-PI/2); + (*link)+=auxPrism; + + // PRISMA IRREGULAR CENTRO ABAJO(Pris_Cen_Ab) + auxPrism=new PrismaticPart; + list.push_back(Vector2D(0,0)); + list.push_back(Vector2D(0.49,0)); + list.push_back(Vector2D(0.49,0.08)); + list.push_back(Vector2D(0.4,0.08)); + list.push_back(Vector2D(0.4,0.27)); + list.push_back(Vector2D(0.33,0.27)); + list.push_back(Vector2D(0.33,0.08)); + list.push_back(Vector2D(0.16,0.08)); + list.push_back(Vector2D(0.16,0.27)); + list.push_back(Vector2D(0.09,0.27)); + list.push_back(Vector2D(0.09,0.08)); + list.push_back(Vector2D(0,0.08)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(255,233,0); + auxPrism->setHeight(0.2); + auxPrism->setRelativePosition(Vector3D(0.38,-0.1,0.03)); + auxPrism->setRelativeOrientation(Y_AXIS,-PI/2); + auxPrism->setRelativeOrientation(Z_AXIS,-PI/2); + (*link)+=auxPrism; + + // PRISMA IRREGULAR CENTRO ARRIBA(Pris_Cen_Ar) + auxPrism=new PrismaticPart; + list.push_back(Vector2D(0,0)); + list.push_back(Vector2D(0.49,0)); + list.push_back(Vector2D(0.49,0.08)); + list.push_back(Vector2D(0,0.08)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(255,233,0); + auxPrism->setHeight(0.2); + auxPrism->setRelativePosition(Vector3D(0.64,-0.1,0.03)); + auxPrism->setRelativeOrientation(Y_AXIS,-PI/2); + auxPrism->setRelativeOrientation(Z_AXIS,-PI/2); + (*link)+=auxPrism; + + // CILINDRO DERECHO ABAJO(Cil_Ab_Der) + auxCyl=new CylindricalPart(0.03,0.175);//Altura y radio + auxCyl->setColor(255,233,0); + auxCyl->setRelativePosition(Vector3D(0,0,0.52)); + (*link)+=auxCyl; + + // PRISMA IRREGULAR DERECHA(Pris_Der) + auxPrism=new PrismaticPart; + list.push_back(Vector2D(-0.13,0)); + list.push_back(Vector2D(0.13,0)); + list.push_back(Vector2D(0.13,1.1)); + list.push_back(Vector2D(-0.13,1.1)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(255,233,0); + auxPrism->setHeight(0.03); + auxPrism->setRelativePosition(Vector3D(0,0,0.52)); + auxPrism->setRelativeOrientation(Z_AXIS,-PI/2); + (*link)+=auxPrism; + + + // CILINDRO DERECHO ARRIBA(Cil_Ar_Der) + auxCyl=new CylindricalPart(0.03,0.175);//Altura y radio + auxCyl->setColor(255,233,0); + auxCyl->setRelativePosition(Vector3D(1.1,0,0.52)); + (*link)+=auxCyl; + + // PRISMA IRREGULAR REFUERZO DERECHA(Pris_Ref_Der) + auxPrism=new PrismaticPart; + list.push_back(Vector2D(-0.4,0)); + list.push_back(Vector2D(0.4,0)); + list.push_back(Vector2D(0.315,0.04)); + list.push_back(Vector2D(-0.315,0.04)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(255,233,0); + auxPrism->setHeight(0.26); + auxPrism->setRelativePosition(Vector3D(0.55,0.13,0.55)); + auxPrism->setRelativeOrientation(X_AXIS,PI/2); + (*link)+=auxPrism; + + //CILINDRO SERVO PEQUEÑO(Servo_Peq) + auxCyl=new CylindricalPart(0.07,0.10);//Altura y radio + auxCyl->setColor(0,0,0); + auxCyl->setRelativePosition(Vector3D(0,0,0.56)); + (*link)+=auxCyl; + + //CILINDRO SERVO GRANDE(Servo_Gran) + auxCyl=new CylindricalPart(0.03,0.175);//Altura y radio + auxCyl->setColor(0,0,0); + auxCyl->setRelativePosition(Vector3D(0,0,0.5)); + (*link)+=auxCyl; + + // AÑADIMOS AL VECTOR LINKS + link->LinkTo(joints[1]); + links.push_back(link); + + + + +//////////////////////////////////////////////////////////////////////////// +//////////////////////////ARTICULACION 3 = LINK[3]////////////////////////// +//////////////////////////////////////////////////////////////////////////// + + //JOINT[2] = ARTICULACION 3 ROTACIONAL + auxJoint=new SimpleJoint(25*PI/36 , -25*PI/36,true,0,Z_AXIS,false); + auxJoint->setRelativePosition(Vector3D(0.18,0,2.1));//Desplazamiento a2 + auxJoint->setRelativeOrientation(0,-PI/2,0); + auxJoint->LinkTo(joints[1]); + auxJoint->setValue(q_init[2]); + joints.push_back(auxJoint); + // joints[2]->setDrawReferenceSystem(true); + + //NUEVO GRUPO DE PIEZAS + link=new ComposedEntity; + link->setName("Link 3"); + +//******************************************************************* // +// LA ARTICULACION 3 ESTA COMPUESTA POR 1 CILINDRO Y 6 PRISMAS IRREG. // +//******************************************************************* // + + // PRISMA IRREGULAR CENTRO ARRIBA(Pris_Cen_Ar) + auxPrism=new PrismaticPart; + list.push_back(Vector2D(-0.16,0)); + list.push_back(Vector2D(0.16,0)); + list.push_back(Vector2D(0.16,0.065)); + list.push_back(Vector2D(-0.16,0.065)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(255,233,0); + auxPrism->setHeight(0.2); + auxPrism->setRelativeOrientation(Y_AXIS,PI/2); + auxPrism->setRelativeOrientation(Z_AXIS,-PI/2); + auxPrism->setRelativePosition(Vector3D(0.56,0.1,0.22)); + (*link)+=auxPrism; + + //CILINDRO(Cilindro_Rojo) + auxCyl=new CylindricalPart(0.03,0.175);//Altura y radio + auxCyl->setColor(255,233,0); + auxCyl->setRelativePosition(Vector3D(0,0,0.38)); + (*link)+=auxCyl; + + // PRISMA IRREGULAR DERECHA(Pris_Der) + auxPrism=new PrismaticPart; + list.push_back(Vector2D(-0.125,0)); + list.push_back(Vector2D(0.125,0)); + list.push_back(Vector2D(0.125,0.97)); + list.push_back(Vector2D(-0.125,0.97)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(255,233,0); + auxPrism->setHeight(0.03); + auxPrism->setRelativeOrientation(Z_AXIS,-PI/2); + auxPrism->setRelativePosition(Vector3D(-0.01,0,0.38)); + (*link)+=auxPrism; + + // PRISMA IRREGULAR IZQUIERDA(Pris_Izq) + auxPrism=new PrismaticPart; + list.push_back(Vector2D(-0.155,0.18)); + list.push_back(Vector2D(0.155,-0.18)); + list.push_back(Vector2D(0.155,0.32)); + list.push_back(Vector2D(0.125,0.38)); + list.push_back(Vector2D(0.125,0.96)); + list.push_back(Vector2D(-0.125,0.96)); + list.push_back(Vector2D(-0.125,0.38)); + list.push_back(Vector2D(-0.155,0.32)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(255,233,0); + auxPrism->setHeight(0.06); + auxPrism->setRelativeOrientation(Z_AXIS,-PI/2); + auxPrism->setRelativePosition(Vector3D(0,0,0)); + (*link)+=auxPrism; + + // PRISMA IRREGULAR CENTRO ABAJO(Pris_Cen_Ab) + auxPrism=new PrismaticPart; + list.push_back(Vector2D(-0.16,0)); + list.push_back(Vector2D(0.16,0)); + list.push_back(Vector2D(0.16,0.065)); + list.push_back(Vector2D(0.1,0.065)); + list.push_back(Vector2D(0.1,0.18)); + list.push_back(Vector2D(0.045,0.18)); + list.push_back(Vector2D(0.045,0.065)); + list.push_back(Vector2D(-0.045,0.065)); + list.push_back(Vector2D(-0.045,0.18)); + list.push_back(Vector2D(-0.1,0.18)); + list.push_back(Vector2D(-0.1,0.065)); + list.push_back(Vector2D(-0.16,0.065)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(255,233,0); + auxPrism->setHeight(0.2); + auxPrism->setRelativeOrientation(Y_AXIS,PI/2); + auxPrism->setRelativeOrientation(Z_AXIS,-PI/2); + auxPrism->setRelativePosition(Vector3D(0.38,0.1,0.22)); + (*link)+=auxPrism; + + // PRISMA IRREGULAR REFUERZO DERECHA(Pris_Ref_Der) + auxPrism=new PrismaticPart; + list.push_back(Vector2D(0,0)); + list.push_back(Vector2D(0.04,0.045)); + list.push_back(Vector2D(0.04,0.81)); + list.push_back(Vector2D(0,0.81)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(255,233,0); + auxPrism->setHeight(0.2); + auxPrism->setRelativeOrientation(Y_AXIS,-PI/2); + auxPrism->setRelativeOrientation(Z_AXIS,-PI/2); + auxPrism->setRelativePosition(Vector3D(0.15,-0.13,0.41)); + (*link)+=auxPrism; + + // PRISMA IRREGULAR ARRIBA(Pris_Ar) + auxPrism=new PrismaticPart; + list.push_back(Vector2D(-0.16,0)); + list.push_back(Vector2D(-0.115,0)); + list.push_back(Vector2D(-0.115,0.05)); + list.push_back(Vector2D(0.115,0.05)); + list.push_back(Vector2D(0.155,0)); + list.push_back(Vector2D(0.16,0)); + list.push_back(Vector2D(0.16,0.11)); + list.push_back(Vector2D(-0.16,0.11)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(255,233,0); + auxPrism->setHeight(0.25); + auxPrism->setRelativeOrientation(X_AXIS,-PI/2); + auxPrism->setRelativeOrientation(Y_AXIS,-PI/2); + auxPrism->setRelativePosition(Vector3D(0.85,-0.13,0.22)); + (*link)+=auxPrism; + + //CILINDRO SERVO GRANDE(Servo_Gran) + auxCyl=new CylindricalPart(0.03,0.175);//Altura y radio + auxCyl->setColor(0,0,0); + auxCyl->setRelativePosition(Vector3D(0,0,-0.08)); + (*link)+=auxCyl; + + //CILINDRO SERVO PEQUEÑO(Servo_Peq) + auxCyl=new CylindricalPart(0.06,0.175);//Altura y radio + auxCyl->setColor(0,0,0); + auxCyl->setRelativePosition(Vector3D(0,0,-0.06)); + (*link)+=auxCyl; + + // SERVO4(Servo4) + auxPrism=new PrismaticPart; + list.push_back(Vector2D(-0.095,-0.2)); + list.push_back(Vector2D(0.095,-0.2)); + list.push_back(Vector2D(0.095,0.2)); + list.push_back(Vector2D(-0.095,0.2)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(0,0,0); + auxPrism->setHeight(0.35); + auxPrism->setRelativePosition(Vector3D(0.07,0,-0.01)); + auxPrism->setRelativeOrientation(Z_AXIS,-PI/2); + (*link)+=auxPrism; + + // SERVO5(Servo5) + auxPrism=new PrismaticPart; + list.push_back(Vector2D(-0.055,-0.11)); + list.push_back(Vector2D(0.055,-0.11)); + list.push_back(Vector2D(0.055,0.11)); + list.push_back(Vector2D(-0.055,0.11)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(0,0,0); + auxPrism->setHeight(0.25); + auxPrism->setRelativePosition(Vector3D(0.67,0,0.23)); + auxPrism->setRelativeOrientation(Y_AXIS,PI/2); + (*link)+=auxPrism; + + //CILINDRO REFUERZO SERVO(Cil_Ref) + auxCyl=new CylindricalPart(0.06,0.08);//Altura y radio + auxCyl->setColor(0,0,0); + auxCyl->setRelativePosition(Vector3D(0.9,-0.15,0.22)); + auxCyl->setRelativeOrientation(Y_AXIS,PI/2); + (*link)+=auxCyl; + + // AÑADIMOS AL VECTOR LINKS + link->LinkTo(joints[2]); + links.push_back(link); + + + + +//////////////////////////////////////////////////////////////////////////// +//////////////////////////ARTICULACION 4 = LINK[4]////////////////////////// +//////////////////////////////////////////////////////////////////////////// + + //JOINT[3] = ARTICULACION 4 ROTACIONAL + auxJoint=new SimpleJoint(7*PI/9 , -7*PI/9,true,0,Z_AXIS,0); + auxJoint->setRelativePosition(Vector3D(-0.05,0.03,3.1)); + auxJoint->LinkTo(joints[2]); + auxJoint->setValue(q_init[3]); + joints.push_back(auxJoint); + // joints[3]->setDrawReferenceSystem(true); + + //NUEVO GRUPO DE PIEZAS + link=new ComposedEntity; + link->setName("Link 4"); + + //******************************************************************** // + // LA ARTICULACION 4 ESTA COMPUESTA POR 2 CILINDROS Y 2 PRISMAS IRREG. // + //******************************************************************** // + + // PRISMA IRREGULAR(Prisma) + auxPrism=new PrismaticPart; + list.push_back(Vector2D(-0.125,0)); + list.push_back(Vector2D(0.2,0)); + list.push_back(Vector2D(0.26,0.03)); + list.push_back(Vector2D(0.29,0.09)); + list.push_back(Vector2D(0.29,0.23)); + list.push_back(Vector2D(0.26,0.275)); + list.push_back(Vector2D(0.23,0.275)); + list.push_back(Vector2D(0.23,0.11)); + list.push_back(Vector2D(0.2,0.11)); + list.push_back(Vector2D(0.2,0.06)); + list.push_back(Vector2D(0.09,0.06)); + list.push_back(Vector2D(0.09,0.03)); + list.push_back(Vector2D(-0.09,0.03)); + list.push_back(Vector2D(-0.09,0.06)); + list.push_back(Vector2D(-0.125,0.06)); + list.push_back(Vector2D(-0.125,0.11)); + list.push_back(Vector2D(-0.155,0.11)); + list.push_back(Vector2D(-0.155,0.13)); + list.push_back(Vector2D(-0.185,0.13)); + list.push_back(Vector2D(-0.21,0.09)); + list.push_back(Vector2D(-0.185,0.03)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(255,233,0); + auxPrism->setHeight(0.25); + auxPrism->setRelativeOrientation(X_AXIS,PI/2); + auxPrism->setRelativeOrientation(Z_AXIS,-PI); + auxPrism->setRelativePosition(Vector3D(0,-0.1,0)); + (*link)+=auxPrism; + + // PRISMA IRREGULAR IZQUIERDA(Pris_Izq) + auxPrism=new PrismaticPart; + list.push_back(Vector2D(-0.045,0)); + list.push_back(Vector2D(0.045,0)); + list.push_back(Vector2D(0.045,0.125)); + list.push_back(Vector2D(-0.045,0.125)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(255,233,0); + auxPrism->setHeight(0.03); + auxPrism->setRelativeOrientation(Y_AXIS,PI/2); + auxPrism->setRelativePosition(Vector3D(0.16,-0.04,0.16)); + (*link)+=auxPrism; + + //CILINDRO IZQUIERDA(Cil_Izq) + auxCyl=new CylindricalPart(0.03,0.065); + auxCyl->setColor(255,233,0); + auxCyl->setRelativePosition(Vector3D(0.16,0.2,0.2)); + auxCyl->setRelativeOrientation(0,PI/2,0); + (*link)+=auxCyl; + + //CILINDRO DERECHA(Cil_Der) + auxCyl=new CylindricalPart(0.03,0.175); + auxCyl->setColor(255,233,0); + auxCyl->setRelativePosition(Vector3D(-0.26,0.02,0.27)); + auxCyl->setRelativeOrientation(0,PI/2,0); + (*link)+=auxCyl; + + + //CILINDRO SERVO ARRIBA(Servo_Ar) + auxCyl=new CylindricalPart(0.09,0.05);//Altura y radio + auxCyl->setColor(0,0,0); + auxCyl->setRelativePosition(Vector3D(-0.25,0.02,0.27)); + auxCyl->setRelativeOrientation(Y_AXIS,PI/2); + (*link)+=auxCyl; + + //CILINDRO SERVO ABAJO(Servo_Ab) + auxCyl=new CylindricalPart(0.05,0.05);//Altura y radio + auxCyl->setColor(0,0,0); + auxCyl->setRelativePosition(Vector3D(0,0,-0.04)); + (*link)+=auxCyl; + + // SERVO6(Servo6) + auxPrism=new PrismaticPart; + list.push_back(Vector2D(-0.055,-0.11)); + list.push_back(Vector2D(0.055,-0.11)); + list.push_back(Vector2D(0.055,0.11)); + list.push_back(Vector2D(-0.055,0.11)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(0,0,0); + auxPrism->setHeight(0.25); + auxPrism->setRelativePosition(Vector3D(-0.24,0.03,0.22)); + auxPrism->setRelativeOrientation(X_AXIS,-PI/2); + auxPrism->setRelativeOrientation(Z_AXIS,PI/2); + (*link)+=auxPrism; + + // AÑADIMOS AL VECTOR LINKS + link->LinkTo(joints[3]); + links.push_back(link); + + + + + +//////////////////////////////////////////////////////////////////////////// +//////////////////////////ARTICULACION 5 = LINK[5]////////////////////////// +//////////////////////////////////////////////////////////////////////////// + + //JOINT[4] = ARTICULACION 5 ROTACIONAL + auxJoint=new SimpleJoint(5*PI/9,-5*PI/9,true,0,Z_AXIS,false); + auxJoint->setRelativePosition(Vector3D(-0.22,0,3.4)); + auxJoint->setRelativeOrientation(PI,-PI/2,0); + auxJoint->LinkTo(joints[3]); + auxJoint->setValue(q_init[4]); + joints.push_back(auxJoint); + // joints[4]->setDrawReferenceSystem(true); + + //NUEVO GRUPO DE PIEZAS + link=new ComposedEntity; + link->setName("Link 5"); + + //******************************************************************* // + // LA ARTICULACION 5 ESTA COMPUESTA POR 3 CILINDROS Y 1 PRISMA IRREG. // + //******************************************************************* // + + // PRISMA IRREGULAR(Prisma) + auxPrism=new PrismaticPart; + list.push_back(Vector2D(-0.17,0)); + list.push_back(Vector2D(0.17,0)); + list.push_back(Vector2D(0.21,0.02)); + list.push_back(Vector2D(0.23,0.06)); + list.push_back(Vector2D(0.23,0.1)); + list.push_back(Vector2D(0.19,0.19)); + list.push_back(Vector2D(0.17,0.19)); + list.push_back(Vector2D(0.17,0.1)); + list.push_back(Vector2D(0.14,0.1)); + list.push_back(Vector2D(0.14,0.06)); + list.push_back(Vector2D(-0.14,0.06)); + list.push_back(Vector2D(-0.14,0.1)); + list.push_back(Vector2D(-0.17,0.1)); + list.push_back(Vector2D(-0.07,0.19)); + list.push_back(Vector2D(-0.19,0.19)); + list.push_back(Vector2D(-0.23,0.1)); + list.push_back(Vector2D(-0.23,0.06)); + list.push_back(Vector2D(-0.21,0.02)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(255,255,0); + auxPrism->setHeight(0.25); + auxPrism->setRelativeOrientation(X_AXIS,PI/2); + auxPrism->setRelativeOrientation(Y_AXIS,-PI/2); + auxPrism->setRelativePosition(Vector3D(-0.19,-0.12,0.19)); + (*link)+=auxPrism; + + // CILINDRO DERECHA(Cil_Der) + auxCyl=new CylindricalPart(0.02,0.12); + auxCyl->setColor(233,233,0); + (*link)+=auxCyl; + + //CILINDRO IZQUIERDA(Cil_Izq) + auxCyl=new CylindricalPart(0.02,0.12); + auxCyl->setColor(233,233,0); + auxCyl->setRelativePosition(Vector3D(0,0,0.36)); + (*link)+=auxCyl; + + + //CILINDRO REFUERZO(Cil_Ref) + auxCyl=new CylindricalPart(0.06,0.08); + auxCyl->setColor(233,233,0); + auxCyl->setRelativePosition(Vector3D(-0.13,0.15,0.20)); + auxCyl->setRelativeOrientation(Y_AXIS,-PI/2); + (*link)+=auxCyl; + + // SERVO7(Servo7) + auxPrism=new PrismaticPart; + list.push_back(Vector2D(-0.055,-0.11)); + list.push_back(Vector2D(0.055,-0.11)); + list.push_back(Vector2D(0.055,0.11)); + list.push_back(Vector2D(-0.055,0.11)); + auxPrism->setPolygonalBase(list); + list.clear(); + auxPrism->setColor(0,0,0); + auxPrism->setHeight(0.25); + auxPrism->setRelativePosition(Vector3D(-0.15,0.05,0.2)); + auxPrism->setRelativeOrientation(Y_AXIS,-PI/2); + (*link)+=auxPrism; + + // AÑADIMOS AL VECTOR LINKS + link->LinkTo(joints[4]); + links.push_back(link); + + + + + +//////////////////////////////////////////////////////////////////////////// +//////////////////////////ARTICULACION 6 = LINK[6]////////////////////////// +//////////////////////////////////////////////////////////////////////////// + + //JOINT[5] = ARTICULACION 6 ROTACIONAL + auxJoint=new SimpleJoint(-PI,PI,true,0,Z_AXIS,false); + auxJoint->setRelativePosition(Vector3D(-0.04,0,3.59)); + auxJoint->LinkTo(joints[4]); + auxJoint->setValue(q_init[5]); + joints.push_back(auxJoint); + // joints[5]->setDrawReferenceSystem(true); + + //NUEVO GRUPO DE PIEZAS + link=new ComposedEntity; + link->setName("Link 6"); + + //************************************************* // + // LA ARTICULACION 6 ESTA COMPUESTA POR UN CILINDRO // + //************************************************* // + // CILINDRO: + auxCyl=new CylindricalPart(0.06,0.03);//Altura y radio + auxCyl->setColor(0,0,0); + auxCyl->setRelativePosition(Vector3D(0,0,0)); + (*link)+=auxCyl; + + // AÑADIMOS AL VECTOR LINKS + auxCyl->LinkTo(joints[5]); + links.push_back(auxCyl); + + + + //Tcp + tcp=new Tcp(); + tcp->setName("Tcp"); + tcp->setRelativePosition(Vector3D(0,0,0.06)); + tcp->LinkTo(joints[5]); + tcp->setDrawReferenceSystem(true); + + getConfigurationOf(q_init,conf); + + //joints[0]->setSimulationParameters(PI/12);// 15º/seg + //joints[1]->setSimulationParameters(23*PI/36);// 115º/seg + //joints[2]->setSimulationParameters(23*PI/36);// 115º/seg + //joints[3]->setSimulationParameters(14*PI/9);// 280º/seg + //joints[4]->setSimulationParameters(5*PI/3);// 300º/seg + //joints[5]->setSimulationParameters(5*PI/3);// 300º/seg + + + (*this)+=links[0]; + + } + + EuitiBotSim::~EuitiBotSim(void) + { + } + + bool EuitiBotSim::getCoordinatesOf(vector &_q) + {//TODO + return true; + } + + + + bool EuitiBotSim::inverseKinematics(Transformation3D t06, vector &_q, unsigned char _conf) + {//TODO + return true; + } + + + + bool EuitiBotSim::getConfigurationOf(const vector &_q, unsigned char &_conf) + {//TODO + return true; + } + + + + bool EuitiBotSim::Pruebas() + { + //TODO + return true; + } + + + + bool EuitiBotSim::configuration(unsigned char _conf, double& _s, double& _e, double& _w) + { + //TODO + return true; + } + + + + bool EuitiBotSim::configuration(double _s, double _e, double _w, unsigned char &_conf) + { + //TODO + return true; + } + + + + + + + void EuitiBotSim::simulate(double delta_t) + { + //TODO + + } + + + void EuitiBotSim::goTo(vector q) + { + //TODO + + } + + + +};//Namespace mr + + diff --git a/src/simulation/EuitiBotSim.h b/src/simulation/EuitiBotSim.h new file mode 100644 index 0000000..8d03037 --- /dev/null +++ b/src/simulation/EuitiBotSim.h @@ -0,0 +1,81 @@ +#pragma once + + +#include "../world/robotsim.h" + + + + +namespace mr +{ + class EuitiBotSim: public RobotSim + { + + DECLARE_MR_OBJECT(EuitiBotSim) + + public: + + //Serializers + virtual void writeToStream(Stream& stream); + virtual void readFromStream(Stream& stream); + virtual void writeToXML(XMLElement* parent); + virtual void readFromXML(XMLElement* parent); + //Constructor + EuitiBotSim(void); + ~EuitiBotSim(void); + + //Cinematica inversa para EuitiBot + virtual bool inverseKinematics(Transformation3D t06, vector &_q, unsigned char _conf=NULL); + + //Return robot configuration + virtual bool getConfigurationOf(const vector &_q, unsigned char &_conf); + bool configuration(unsigned char _conf, double& _s, double& _e, double& _w); + bool configuration(double _s, double _e, double _w,unsigned char &_conf); + + //Simulate + void simulate(double delta_t); + + //Aux. methods + void setFlash(); + bool getCoordinatesOf(vector &_q);// delete? + bool Pruebas();//delete + + protected: + + //Cinematica inversa para EuitiBot + //bool EuitiBotinverseKinematics(Transformation3D t06, vector &_q, unsigned char _conf=0x00); + void goTo(vector _q); + + + /*** + Ranges coordiantes: + joints[0] -> 360º ->[PI/2 , -PI/2] + joints[1] -> 250º ->[25*PI/36 , -25*PI/36] 125º / -125º + joints[2] -> 250º ->[25*PI/36 , -25*PI/36] + joints[3] -> 360º ->[PI/2 , -PI/2] + joints[4] -> 250º ->[25*PI/36 , -25*PI/36] + joints[5] -> 360º ->[PI/2 , -PI/2] + ***/ + + + private: + + + unsigned char conf; + vector q_init; + vector q_target; + + //cinematic simulation atributes + vector a0,a1,a2,a3; + double time; + double targetTime; + + + + }; + +}; + + + + diff --git a/src/simulation/adeptonesim.cpp b/src/simulation/adeptonesim.cpp index 4709c98..cc0a7c6 100644 --- a/src/simulation/adeptonesim.cpp +++ b/src/simulation/adeptonesim.cpp @@ -41,6 +41,12 @@ void AdeptOneSim::writeToStream(Stream& stream) {SolidEntity::writeToStream(stream);} void AdeptOneSim::readFromStream(Stream& stream) {SolidEntity::readFromStream(stream);} + +void AdeptOneSim::writeToXML(XMLElement* parent) + {SolidEntity::writeToXML(parent);} +void AdeptOneSim::readFromXML(XMLElement* parent) + {SolidEntity::readFromXML(parent);} + AdeptOneSim::AdeptOneSim() { name="SCARA Adept One Sim"; @@ -119,6 +125,11 @@ Codo izquierdo -> elbow = -1; SimpleJoint *auxJoint=new SimpleJoint(5*PI/6,-5*PI/6,true,0,Z_AXIS,false); auxJoint->setRelativePosition(Vector3D(0,0,0.99)); auxJoint->LinkTo(links[0]); + + actuator=new Actuator(); + actuator->linkTo(auxJoint); + actuators.push_back(actuator); + joints.push_back(auxJoint); //Arm 1 link=new ComposedEntity; @@ -184,6 +195,11 @@ Codo izquierdo -> elbow = -1; auxJoint=new SimpleJoint(49*PI/60,-49*PI/60,true,0,Z_AXIS,false); auxJoint->setRelativePosition(Vector3D(0.425,0,0)); auxJoint->LinkTo(joints[0]); + + actuator=new Actuator(); + actuator->linkTo(auxJoint); + actuators.push_back(actuator); + joints.push_back(auxJoint); //Arm 2 link=new ComposedEntity; @@ -236,6 +252,11 @@ Codo izquierdo -> elbow = -1; auxJoint=new SimpleJoint(0,-0.5,true,0,Z_AXIS,true);//Prismatica auxJoint->setRelativePosition(Vector3D(0.375,0,0)); auxJoint->LinkTo(joints[1]); + + actuator=new Actuator(); + actuator->linkTo(auxJoint); + actuators.push_back(actuator); + joints.push_back(auxJoint); //Arm 3 @@ -250,6 +271,11 @@ Codo izquierdo -> elbow = -1; //joint 3 auxJoint=new SimpleJoint(277*PI/180,-277*PI/180,true,0,Z_AXIS,false); auxJoint->LinkTo(joints[2]); + + actuator=new Actuator(); + actuator->linkTo(auxJoint); + actuators.push_back(actuator); + joints.push_back(auxJoint); //Arm 4 auxCyl=new CylindricalPart(0.02,0.05); @@ -271,6 +297,18 @@ Codo izquierdo -> elbow = -1; setJointValue(0,-0.44); setJointValue(0,0.75398); + + + actuators[0]->setSimulationParameters(23*PI/12);// 115º/seg Moidficated->before was 15º/seg + actuators[1]->setSimulationParameters(5*PI/3);// 300º/seg + actuators[2]->setSimulationParameters(0.5);// 500 mm/seg + actuators[3]->setSimulationParameters(50*PI/9);// 1000º/seg + +//Ficha técnica Máx speed + //Joint 1 540°/sec + //Joint 2 540°/sec + //Joint 3 500 mm/sec (19.7 in./sec) + //Joint 4 3600°/sec } bool AdeptOneSim::ADEPTONEinverseKinematics(double yaw,Vector3D p,vector &_q,unsigned char conf) @@ -388,6 +426,10 @@ bool AdeptOneSim::getConfigurationOf(const vector &_q, unsigned char &c +//void AdeptOneSim::simulate(double delta_t) +//{ +// +//} diff --git a/src/simulation/adeptonesim.h b/src/simulation/adeptonesim.h index 20a4b0e..512bc54 100644 --- a/src/simulation/adeptonesim.h +++ b/src/simulation/adeptonesim.h @@ -52,6 +52,8 @@ class AdeptOneSim : public RobotSim //Serializers virtual void writeToStream(Stream& stream); virtual void readFromStream(Stream& stream); + virtual void writeToXML(XMLElement* parent); + virtual void readFromXML(XMLElement* parent); //Constructor AdeptOneSim(void); @@ -64,7 +66,8 @@ class AdeptOneSim : public RobotSim //Forward and inverse SCARA_ADEPT_ONE kinematics Relative virtual bool inverseKinematics(Transformation3D t, vector &_q, unsigned char conf=0);//Not move robot - +//simulation method + //void simulate(double delta_t); protected: //Specific inverse methode of SCARA_ADEPT_ONE diff --git a/src/simulation/aseairb2000sim.cpp b/src/simulation/aseairb2000sim.cpp index 5b24a78..69de0c1 100644 --- a/src/simulation/aseairb2000sim.cpp +++ b/src/simulation/aseairb2000sim.cpp @@ -50,6 +50,11 @@ void AseaIRB2000Sim::writeToStream(Stream& stream) void AseaIRB2000Sim::readFromStream(Stream& stream) {SolidEntity::readFromStream(stream);} +void AseaIRB2000Sim::writeToXML(XMLElement* parent) + {SolidEntity::writeToXML(parent);} +void AseaIRB2000Sim::readFromXML(XMLElement* parent) + {SolidEntity::readFromXML(parent);} + AseaIRB2000Sim::AseaIRB2000Sim() { name="ASEA IRB 2000"; @@ -106,6 +111,11 @@ AseaIRB2000Sim::AseaIRB2000Sim() //Joint[0] SimpleJoint *auxJoint=new SimpleJoint(PI,-PI,true,0,Z_AXIS,false); auxJoint->LinkTo(links[0]); + + actuator=new Actuator(); + actuator->linkTo(auxJoint); + actuators.push_back(actuator); + auxJoint->setValue(q_init[0]); joints.push_back(auxJoint); // joints[0]->setDrawReferenceSystem(true); @@ -209,6 +219,11 @@ AseaIRB2000Sim::AseaIRB2000Sim() auxJoint->setRelativePosition(Vector3D(0,0,0.78)); auxJoint->setRelativeOrientation(X_AXIS,-PI/2); auxJoint->LinkTo(joints[0]); + + actuator=new Actuator(); + actuator->linkTo(auxJoint); + actuators.push_back(actuator); + auxJoint->setValue(q_init[1]); joints.push_back(auxJoint); // joints[1]->setDrawReferenceSystem(true); @@ -301,6 +316,11 @@ AseaIRB2000Sim::AseaIRB2000Sim() auxJoint->setRelativePosition(Vector3D(0,-0.71,0)); auxJoint->setRelativeOrientation(Z_AXIS,PI/2); auxJoint->LinkTo(joints[1]); + + actuator=new Actuator(); + actuator->linkTo(auxJoint); + actuators.push_back(actuator); + auxJoint->setValue(q_init[2]); joints.push_back(auxJoint); // joints[2]->setDrawReferenceSystem(true); @@ -371,6 +391,11 @@ AseaIRB2000Sim::AseaIRB2000Sim() auxJoint->setRelativePosition(Vector3D(-0.125,0,0)); auxJoint->setRelativeOrientation(X_AXIS,PI/2); auxJoint->LinkTo(joints[2]); + + actuator=new Actuator(); + actuator->linkTo(auxJoint); + actuators.push_back(actuator); + auxJoint->setValue(q_init[3]); joints.push_back(auxJoint); // joints[3]->setDrawReferenceSystem(true); @@ -493,6 +518,11 @@ AseaIRB2000Sim::AseaIRB2000Sim() auxJoint->setRelativePosition(Vector3D(0,0,0.85)); auxJoint->setRelativeOrientation(X_AXIS,-PI/2); auxJoint->LinkTo(joints[3]); + + actuator=new Actuator(); + actuator->linkTo(auxJoint); + actuators.push_back(actuator); + auxJoint->setValue(q_init[4]); joints.push_back(auxJoint); // joints[4]->setDrawReferenceSystem(true); @@ -544,6 +574,11 @@ AseaIRB2000Sim::AseaIRB2000Sim() auxJoint=new SimpleJoint(10*PI/9 , -10*PI/9,true,0,Z_AXIS,0); auxJoint->setRelativeOrientation(X_AXIS,PI/2); auxJoint->LinkTo(joints[4]); + + actuator=new Actuator(); + actuator->linkTo(auxJoint); + actuators.push_back(actuator); + auxJoint->setValue(q_init[5]); joints.push_back(auxJoint); // joints[5]->setDrawReferenceSystem(true); @@ -569,12 +604,20 @@ AseaIRB2000Sim::AseaIRB2000Sim() (*this)+=links[0]; //Speed joints in rad/seg. - joints[0]->setSimulationParameters(PI/12);// 15º/seg - joints[1]->setSimulationParameters(23*PI/36);// 115º/seg - joints[2]->setSimulationParameters(23*PI/36);// 115º/seg - joints[3]->setSimulationParameters(14*PI/9);// 280º/seg - joints[4]->setSimulationParameters(5*PI/3);// 300º/seg - joints[5]->setSimulationParameters(5*PI/3);// 300º/seg + actuators[0]->setSimulationParameters(23*PI/12);// 115º/seg Moidficated->before was 15º/seg + actuators[1]->setSimulationParameters(23*PI/36);// 115º/seg + actuators[2]->setSimulationParameters(23*PI/36);// 115º/seg + actuators[3]->setSimulationParameters(14*PI/9);// 280º/seg + actuators[4]->setSimulationParameters(5*PI/3);// 300º/seg + actuators[5]->setSimulationParameters(5*PI/3);// 300º/seg + +//Ficha técnica AseaIrb2000 Max speed + //Axis 1 150°/s + //Axis 2 150°/s + //Axis 3 150°/s + //Axis 4 360°/s + //Axis 5 360°/s + //Axis 6 450°/s } @@ -1107,98 +1150,13 @@ bool AseaIRB2000Sim::configuration(double _f, double _e, double _w, unsigned cha return true; } -void AseaIRB2000Sim::simulate(double delta_t) -{ - ASEAIRB2000Mechanism(); - if((int)q_target.size() == (int)joints.size()) - { - double error=0; - for(int i=0;i<(int)joints.size();i++) - error+=fabs(q_target[i]-joints[i]->getValue());//* - if( (time+delta_t) >= targetTime)//* - { - for(int i=0;i<(int)joints.size();i++) - joints[i]->setTarget(q_target[i]); - q_target.clear(); - time=0.0; - targetTime=0.0; - a0.clear();a1.clear();a2.clear();a3.clear(); - } - else - { - for(int i=0;i<(int)joints.size();i++) - { - joints[i]->setTarget(a0[i] + a1[i]*time + a2[i]*time*time + a3[i]*time*time*time); - joints[i]->setSpeed(a1[i] + 2*a2[i]*time + 3*a3[i]*time*time); - } - time+=delta_t; - } - } - RobotSim::simulate(delta_t); - - return; -} - -void AseaIRB2000Sim::goToAbs(Transformation3D t) -{ - vector q; - if(!inverseKinematicsAbs(t,q))return; - goTo(q); - return; -} - -void AseaIRB2000Sim::goTo(Transformation3D t) -{ - vector q; - if(!inverseKinematics(t,q))return; - goTo(q); - return; -} - -void AseaIRB2000Sim::goTo(vector q) +void AseaIRB2000Sim::simulate(double delta_t) { - q_target=q;//Target loaded - - double error=0; - for(int i=0;i<(int)joints.size();i++) - error+=fabs(q_target[i] - joints[i]->getValue());//* - - if(fabs(error) < EPS) - { - q_target.clear(); - return; - } - else - { - double lowestSpeed=100.0; - double longestPath=0.0; - vector path; - - for(int i=0;i<(int)joints.size();i++) - path.push_back(q_target[i] - joints[i]->getValue());//Path each coordinate: target minus current coordinates - - for(int i=0;i<(int)joints.size();i++) - { - if(joints[i]->getMaxSpeed() <= lowestSpeed) - lowestSpeed = joints[i]->getMaxSpeed(); - if(fabs(path[i]) >= longestPath) - longestPath = fabs(path[i]);//* - } - targetTime = longestPath / lowestSpeed; - time=0.0; - for(int i=0;i<(int)joints.size();i++) - { - a0.push_back(joints[i]->getValue());//Current coordiantes - a1.push_back(0); - a2.push_back( 3*(path[i])/(square(targetTime))); - a3.push_back(-2*(path[i])/(square(targetTime)*targetTime)); - } - } - - return; + ASEAIRB2000Mechanism(); + RobotSim::simulate(delta_t); } };//Namespace mr diff --git a/src/simulation/aseairb2000sim.h b/src/simulation/aseairb2000sim.h index e74b121..fa6f2ca 100644 --- a/src/simulation/aseairb2000sim.h +++ b/src/simulation/aseairb2000sim.h @@ -53,6 +53,8 @@ class AseaIRB2000Sim : public RobotSim //Serializers virtual void writeToStream(Stream& stream); virtual void readFromStream(Stream& stream); + virtual void writeToXML(XMLElement* parent); + virtual void readFromXML(XMLElement* parent); //Constructor AseaIRB2000Sim(void); @@ -69,13 +71,11 @@ class AseaIRB2000Sim : public RobotSim bool configuration(double _f, double _e, double _w,unsigned char &_conf); void ASEAIRB2000Mechanism(); -//simulation methods +//simulation methods specific public: - virtual void simulate(double delta_t); - void goToAbs(Transformation3D t); - //Load T3D relative - void goTo(Transformation3D t); - void goTo(vector _q); + void simulate(double delta_t); + + protected: //Specific inverse methode of Asea IRB 2000 @@ -92,15 +92,8 @@ Ranges coordiantes: ***/ private: - unsigned char conf; SimpleJoint *q_bar[3]; - vector q_init; - vector q_target; - //cinematic simulation atributes - vector a0,a1,a2,a3; - double time; - double targetTime; }; };//end namespace mr diff --git a/src/simulation/camerasim.cpp b/src/simulation/camerasim.cpp index 1617da0..b3082f6 100644 --- a/src/simulation/camerasim.cpp +++ b/src/simulation/camerasim.cpp @@ -47,6 +47,16 @@ void CameraSim::readFromStream(Stream& stream) SolidEntity::readFromStream(stream); } +void CameraSim::writeToXML(XMLElement* parent) +{ + SolidEntity::writeToXML(parent); +} + +void CameraSim::readFromXML(XMLElement* parent) +{ + SolidEntity::readFromXML(parent); +} + ostream& operator<<(ostream& os, const CameraSim& p) { //os<>startAngle>>stepAngle>>numSteps>>maxRange>>sigma; setLaserProperties(startAngle,stepAngle,numSteps,maxRange,sigma); } +void LaserSensorSim::writeToXML(XMLElement* parent) +{ + +// XMLAux aux; +// XMLElement* lasersensor=new XMLElement(parent,"laserSensorSim"); + XMLVariable* startAn= new XMLVariable ("startAngle",XMLAux::string_Convert(startAngle).c_str()); + XMLVariable* stepAn= new XMLVariable ("stepAngle",XMLAux::string_Convert(stepAngle).c_str()); + XMLVariable* _numStep= new XMLVariable ("numSteps",XMLAux::string_Convert(numSteps).c_str()); + XMLVariable* _maxRange= new XMLVariable ("maxRange",XMLAux::string_Convert(maxRange).c_str()); + XMLVariable* _sigma= new XMLVariable ("sigma",XMLAux::string_Convert(sigma).c_str()); + + parent->AddVariable(startAn); + parent->AddVariable(stepAn); + parent->AddVariable(_numStep); + parent->AddVariable(_maxRange); + parent->AddVariable(_sigma); + //parent->AddElement(lasersensor); + + SolidEntity::writeToXML(parent); + +} + +void LaserSensorSim::readFromXML(XMLElement* parent) +{ + double startAn=startAngle, stepAn=stepAngle, maxR=maxRange, _sigma=sigma; + int _numSteps=numSteps; + + if (parent->FindVariableZ("startAngle")) + { + startAn=XMLAux::GetValueDouble(parent->FindVariableZ("startAngle")); + } + if (parent->FindVariableZ("stepAngle")) + { + stepAn=XMLAux::GetValueDouble(parent->FindVariableZ("stepAngle")); + } + if (parent->FindVariableZ("numSteps")) + { + _numSteps=parent->FindVariableZ("numSteps")->GetValueInt(); + } + + if (parent->FindVariableZ("maxRange")) + { + maxR=XMLAux::GetValueDouble(parent->FindVariableZ("maxRange")); + } + if (parent->FindVariableZ("sigma")) + { + _sigma=XMLAux::GetValueDouble(parent->FindVariableZ("sigma")); + } + + setLaserProperties(startAn,stepAn,_numSteps,maxR,_sigma); + + SolidEntity::readFromXML(parent); + +} ostream& operator<<(ostream& os, const LaserSensorSim& p) { diff --git a/src/simulation/lasersensorsim.h b/src/simulation/lasersensorsim.h index e29f5d0..dc72843 100644 --- a/src/simulation/lasersensorsim.h +++ b/src/simulation/lasersensorsim.h @@ -40,6 +40,8 @@ //#include "../math/segment3d.h" #include "../world/world.h" #include "../system/mutex.h" +#include "../base/xmlaux.h" + using namespace std; @@ -93,6 +95,8 @@ class LaserSensorSim : public LaserSensor, public ComposedEntity //serialization virtual void writeToStream(Stream& stream); virtual void readFromStream(Stream& stream); + virtual void writeToXML(XMLElement* parent); + virtual void readFromXML(XMLElement* parent); //set laser properties void setLaserProperties(double _startangle, double _step, int _numsteps, double _maxrange, double _sigma); diff --git a/src/simulation/lms100sim.cpp b/src/simulation/lms100sim.cpp index ff2d6c3..a28a1cc 100644 --- a/src/simulation/lms100sim.cpp +++ b/src/simulation/lms100sim.cpp @@ -61,5 +61,13 @@ void LMS100Sim::readFromStream(Stream& stream) { SolidEntity::readFromStream(stream); } +void LMS100Sim::writeToXML(XMLElement* parent) +{ + SolidEntity::writeToXML(parent); +} +void LMS100Sim::readFromXML(XMLElement* parent) +{ + SolidEntity::readFromXML(parent); +} }; //Namespace mr diff --git a/src/simulation/lms100sim.h b/src/simulation/lms100sim.h index a9f795a..3a167ad 100644 --- a/src/simulation/lms100sim.h +++ b/src/simulation/lms100sim.h @@ -46,6 +46,8 @@ class LMS100Sim :public LaserSensorSim //Serializers void writeToStream(Stream& stream); void readFromStream(Stream& stream); + void writeToXML(XMLElement* parent); + void readFromXML(XMLElement* parent); virtual void drawGL(); }; diff --git a/src/simulation/lms200sim.cpp b/src/simulation/lms200sim.cpp index 9a95689..95e234d 100644 --- a/src/simulation/lms200sim.cpp +++ b/src/simulation/lms200sim.cpp @@ -60,5 +60,14 @@ void LMS200Sim::readFromStream(Stream& stream) { SolidEntity::readFromStream(stream); } +void LMS200Sim::writeToXML(XMLElement* parent) +{ + SolidEntity::writeToXML(parent); +} + +void LMS200Sim::readFromXML(XMLElement* parent) +{ + SolidEntity::readFromXML(parent); +} }; //Namespace mr diff --git a/src/simulation/lms200sim.h b/src/simulation/lms200sim.h index db98f74..3657c9f 100644 --- a/src/simulation/lms200sim.h +++ b/src/simulation/lms200sim.h @@ -46,6 +46,8 @@ class LMS200Sim :public LaserSensorSim //Serializers void writeToStream(Stream& stream); void readFromStream(Stream& stream); + void writeToXML(XMLElement* parent); + void readFromXML(XMLElement* parent); virtual void drawGL(); }; diff --git a/src/simulation/nemolasersensor3dsim.cpp b/src/simulation/nemolasersensor3dsim.cpp index 365fa17..49b4b6b 100644 --- a/src/simulation/nemolasersensor3dsim.cpp +++ b/src/simulation/nemolasersensor3dsim.cpp @@ -30,6 +30,16 @@ void NemoLaserSensor3DSim::readFromStream(Stream& stream) { SolidEntity::readFromStream(stream); } +void NemoLaserSensor3DSim::writeToXML(XMLElement* parent) +{ + SolidEntity::writeToXML(parent); +} + +void NemoLaserSensor3DSim::readFromXML(XMLElement* parent) +{ + SolidEntity::readFromXML(parent); +} + void NemoLaserSensor3DSim::drawGL() { //it si possible to make the drawing independent of the geometric model diff --git a/src/simulation/nemolasersensor3dsim.h b/src/simulation/nemolasersensor3dsim.h index 729b4b2..ccbc52d 100644 --- a/src/simulation/nemolasersensor3dsim.h +++ b/src/simulation/nemolasersensor3dsim.h @@ -52,6 +52,8 @@ class NemoLaserSensor3DSim :public LaserSensor3DSim //Serializers virtual void writeToStream(Stream& stream); virtual void readFromStream(Stream& stream); + virtual void writeToXML(XMLElement* parent); + virtual void readFromXML(XMLElement* parent); virtual void drawGL(); diff --git a/src/simulation/patrolbotsim.cpp b/src/simulation/patrolbotsim.cpp index 149aa4d..d32a063 100644 --- a/src/simulation/patrolbotsim.cpp +++ b/src/simulation/patrolbotsim.cpp @@ -93,6 +93,17 @@ objects[5]->setRelativePosition(Vector3D(-large/2,-width/2,wheel_radius+0.001)); } void PatrolbotSim::writeToStream(Stream& stream) {WheeledBaseSim::writeToStream(stream);} + void PatrolbotSim::readFromStream(Stream& stream) {WheeledBaseSim::readFromStream(stream);} + +void PatrolbotSim::writeToXML(XMLElement* parent) +{ + WheeledBaseSim::writeToXML(parent); +} + +void PatrolbotSim::readFromXML(XMLElement* parent) +{ + WheeledBaseSim::readFromXML(parent); +} }; //Namespace mr diff --git a/src/simulation/patrolbotsim.h b/src/simulation/patrolbotsim.h index 78353fb..3a0a23a 100644 --- a/src/simulation/patrolbotsim.h +++ b/src/simulation/patrolbotsim.h @@ -46,6 +46,9 @@ class PatrolbotSim :public WheeledBaseSim //Serializers virtual void writeToStream(Stream& stream); virtual void readFromStream(Stream& stream); + virtual void writeToXML(XMLElement* parent); + virtual void readFromXML(XMLElement* parent); + virtual void simulate(double delta_t); virtual void drawGL(); }; diff --git a/src/simulation/personsim.cpp b/src/simulation/personsim.cpp index 3a6e826..0979dbe 100644 --- a/src/simulation/personsim.cpp +++ b/src/simulation/personsim.cpp @@ -50,6 +50,15 @@ void PersonSim::writeToStream(Stream& stream) void PersonSim::readFromStream(Stream& stream) { +} +void PersonSim::writeToXML(XMLElement* parent) +{ + +} + +void PersonSim::readFromXML(XMLElement* parent) +{ + } void PersonSim::drawGL() { diff --git a/src/simulation/personsim.h b/src/simulation/personsim.h index b5a7478..efbbe1a 100644 --- a/src/simulation/personsim.h +++ b/src/simulation/personsim.h @@ -46,6 +46,9 @@ class PersonSim: public ComposedEntity //Serializers virtual void writeToStream(Stream& stream); virtual void readFromStream(Stream& stream); + virtual void writeToXML(XMLElement* parent); + virtual void readFromXML(XMLElement* parent); + virtual void simulate(double delta_t); virtual void drawGL(); diff --git a/src/simulation/pioneer3atsim.cpp b/src/simulation/pioneer3atsim.cpp index e1365e1..a318c0b 100644 --- a/src/simulation/pioneer3atsim.cpp +++ b/src/simulation/pioneer3atsim.cpp @@ -70,8 +70,19 @@ wheel4->setRelativePosition(Vector3D(-large/2,-width/2,wheel_radius+0.001)); } void Pioneer3ATSim::writeToStream(Stream& stream) {WheeledBaseSim::writeToStream(stream);} + void Pioneer3ATSim::readFromStream(Stream& stream) {WheeledBaseSim::readFromStream(stream);} + +void Pioneer3ATSim::writeToXML(XMLElement* parent) +{ + WheeledBaseSim::writeToXML(parent); +} + +void Pioneer3ATSim::readFromXML(XMLElement* parent) +{ + WheeledBaseSim::readFromXML(parent); +} void Pioneer3ATSim::drawGL() { //it si possible to make the drawing independent of the geometric model diff --git a/src/simulation/pioneer3atsim.h b/src/simulation/pioneer3atsim.h index 254b26c..f8f0161 100644 --- a/src/simulation/pioneer3atsim.h +++ b/src/simulation/pioneer3atsim.h @@ -50,6 +50,9 @@ class Pioneer3ATSim :public WheeledBaseSim //Serializers virtual void writeToStream(Stream& stream); virtual void readFromStream(Stream& stream); + virtual void writeToXML(XMLElement* parent); + virtual void readFromXML(XMLElement* parent); + virtual void simulate(double delta_t); virtual void drawGL(); }; diff --git a/src/simulation/powercube70sim.cpp b/src/simulation/powercube70sim.cpp index 41236f8..b16627c 100644 --- a/src/simulation/powercube70sim.cpp +++ b/src/simulation/powercube70sim.cpp @@ -85,6 +85,16 @@ void PowerCube70Sim::readFromStream(Stream& stream){ stream>>p>>t; setPos(p,t); } +void PowerCube70Sim::writeToXML(XMLElement* parent) +{ + SolidEntity::writeToXML(parent); +} +void PowerCube70Sim::readFromXML(XMLElement* parent) +{ + SolidEntity::readFromXML(parent); +} + + void PowerCube70Sim::simulate(double delta_t) { //aqui es necesario recoger el mundo al que pertenece el world diff --git a/src/simulation/powercube70sim.h b/src/simulation/powercube70sim.h index f5f8a34..d86ac83 100644 --- a/src/simulation/powercube70sim.h +++ b/src/simulation/powercube70sim.h @@ -47,6 +47,8 @@ class PowerCube70Sim : public PowerCube70, public ComposedEntity //Serializers virtual void writeToStream(Stream& stream); virtual void readFromStream(Stream& stream); + virtual void writeToXML(XMLElement* parent); + virtual void readFromXML(XMLElement* parent); //Constructor PowerCube70Sim(); diff --git a/src/simulation/puma560sim.cpp b/src/simulation/puma560sim.cpp index 1689030..26e1a24 100644 --- a/src/simulation/puma560sim.cpp +++ b/src/simulation/puma560sim.cpp @@ -48,6 +48,14 @@ void Puma560Sim::writeToStream(Stream& stream) {SolidEntity::writeToStream(stream);} void Puma560Sim::readFromStream(Stream& stream) {SolidEntity::readFromStream(stream);} +void Puma560Sim::writeToXML(XMLElement* parent) +{ + SolidEntity::writeToXML(parent); +} +void Puma560Sim::readFromXML(XMLElement* parent) +{ + SolidEntity::readFromXML(parent); +} Puma560Sim::Puma560Sim() { @@ -83,6 +91,11 @@ Puma560Sim::Puma560Sim() //Joint[0] SimpleJoint *auxJoint=new SimpleJoint(8*PI/9 , -8*PI/9,true,0,Z_AXIS,false); auxJoint->LinkTo(links[0]); + + actuator=new Actuator(); + actuator->linkTo(auxJoint); + actuators.push_back(actuator); + auxJoint->setValue(q_init[0]); joints.push_back(auxJoint); // joints[0]->setDrawReferenceSystem(true); @@ -140,6 +153,11 @@ Puma560Sim::Puma560Sim() auxJoint->setRelativePosition(Vector3D(0.15,0,0.66));//Desplazamiento d2 auxJoint->setRelativeOrientation(PI/2,-PI/2,PI/2); auxJoint->LinkTo(joints[0]); + + actuator=new Actuator(); + actuator->linkTo(auxJoint); + actuators.push_back(actuator); + auxJoint->setValue(q_init[1]); joints.push_back(auxJoint); // joints[1]->setDrawReferenceSystem(true); @@ -186,6 +204,11 @@ podria dejar asi, pero no tiene mucho sentido la composicion de un unico objeto. auxJoint=new SimpleJoint(3*PI/4,-3*PI/4,true,0,Z_AXIS,false); auxJoint->setRelativePosition(Vector3D(0.432,0,0));//Desplazamiento a2 auxJoint->LinkTo(joints[1]); + + actuator=new Actuator(); + actuator->linkTo(auxJoint); + actuators.push_back(actuator); + auxJoint->setValue(q_init[2]); joints.push_back(auxJoint); // joints[2]->setDrawReferenceSystem(true); @@ -233,6 +256,11 @@ podria dejar asi, pero no tiene mucho sentido la composicion de un unico objeto. auxJoint->setRelativePosition(Vector3D(0.36,-0.02,0)); auxJoint->setRelativeOrientation(PI,-PI/2,0); auxJoint->LinkTo(joints[2]); + + actuator=new Actuator(); + actuator->linkTo(auxJoint); + actuators.push_back(actuator); + auxJoint->setValue(q_init[3]); joints.push_back(auxJoint); // joints[3]->setDrawReferenceSystem(true); @@ -280,6 +308,11 @@ podria dejar asi, pero no tiene mucho sentido la composicion de un unico objeto. //0.36 hasta los 0.432 auxJoint->setRelativeOrientation(PI,-PI/2,0); auxJoint->LinkTo(joints[3]); + + actuator=new Actuator(); + actuator->linkTo(auxJoint); + actuators.push_back(actuator); + auxJoint->setValue(q_init[4]); joints.push_back(auxJoint); // joints[4]->setDrawReferenceSystem(true); @@ -307,6 +340,11 @@ podria dejar asi, pero no tiene mucho sentido la composicion de un unico objeto. auxJoint->setRelativePosition(Vector3D(0.055,0,0)); auxJoint->setRelativeOrientation(PI/2,0,PI/2); auxJoint->LinkTo(joints[4]); + + actuator=new Actuator(); + actuator->linkTo(auxJoint); + actuators.push_back(actuator); + auxJoint->setValue(q_init[5]); joints.push_back(auxJoint); // joints[5]->setDrawReferenceSystem(true); @@ -330,12 +368,14 @@ podria dejar asi, pero no tiene mucho sentido la composicion de un unico objeto. getConfigurationOf(q_init,conf); - joints[0]->setSimulationParameters(PI/12);// 15º/seg - joints[1]->setSimulationParameters(23*PI/36);// 115º/seg - joints[2]->setSimulationParameters(23*PI/36);// 115º/seg - joints[3]->setSimulationParameters(14*PI/9);// 280º/seg - joints[4]->setSimulationParameters(5*PI/3);// 300º/seg - joints[5]->setSimulationParameters(5*PI/3);// 300º/seg + + + actuators[0]->setSimulationParameters(23*PI/12);// 115º/seg Moidficated->before was 15º/seg + actuators[1]->setSimulationParameters(23*PI/36);// 115º/seg + actuators[2]->setSimulationParameters(23*PI/36);// 115º/seg + actuators[3]->setSimulationParameters(14*PI/9);// 280º/seg + actuators[4]->setSimulationParameters(5*PI/3);// 300º/seg + actuators[5]->setSimulationParameters(5*PI/3);// 300º/seg (*this)+=links[0]; @@ -568,78 +608,12 @@ void Puma560Sim::setFlash() } } + void Puma560Sim::simulate(double delta_t) { setFlash(); - - if((int)q_target.size() == (int)joints.size()) - { - double error=0; - for(int i=0;i<(int)joints.size();i++) - error+=fabs(q_target[i]-joints[i]->getValue());//* - - if( (time+delta_t) >= targetTime)//* - { - for(int i=0;i<(int)joints.size();i++) - joints[i]->setTarget(q_target[i]); - q_target.clear(); - time=0.0; - targetTime=0.0; - a0.clear();a1.clear();a2.clear();a3.clear(); - } - else - { - for(int i=0;i<(int)joints.size();i++) - { - joints[i]->setTarget(a0[i] + a1[i]*time + a2[i]*time*time + a3[i]*time*time*time); - joints[i]->setSpeed(a1[i] + 2*a2[i]*time + 3*a3[i]*time*time); - } - time+=delta_t; - } - } - RobotSim::simulate(delta_t); -} -void Puma560Sim::goTo(vector q) -{ - q_target=q;//Target loaded - - double error=0; - for(int i=0;i<(int)joints.size();i++) - error+=fabs(q_target[i] - joints[i]->getValue());//* - if(fabs(error) < EPS) - { - q_target.clear(); - return; - } - else - { - double lowestSpeed=100.0; - double longestPath=0.0; - vector path; - - for(int i=0;i<(int)joints.size();i++) - path.push_back(q_target[i] - joints[i]->getValue());//Path each coordinate: target minus current coordinates - - for(int i=0;i<(int)joints.size();i++) - { - if(joints[i]->getMaxSpeed() <= lowestSpeed) - lowestSpeed = joints[i]->getMaxSpeed(); - if(fabs(path[i]) >= longestPath) - longestPath = fabs(path[i]);//* - } - targetTime = longestPath / lowestSpeed; - time=0.0; - for(int i=0;i<(int)joints.size();i++) - { - a0.push_back(joints[i]->getValue());//Current coordiantes - a1.push_back(0); - a2.push_back( 3*(path[i])/(square(targetTime))); - a3.push_back(-2*(path[i])/(square(targetTime)*targetTime)); - } - } - - return; } + };//Namespace mr diff --git a/src/simulation/puma560sim.h b/src/simulation/puma560sim.h index acab413..a9e3244 100644 --- a/src/simulation/puma560sim.h +++ b/src/simulation/puma560sim.h @@ -53,6 +53,8 @@ class Puma560Sim : public RobotSim //Serializers virtual void writeToStream(Stream& stream); virtual void readFromStream(Stream& stream); + virtual void writeToXML(XMLElement* parent); + virtual void readFromXML(XMLElement* parent); //Constructor Puma560Sim(void); @@ -65,7 +67,7 @@ class Puma560Sim : public RobotSim bool configuration(unsigned char _conf, double& _s, double& _e, double& _w); bool configuration(double _s, double _e, double _w,unsigned char &_conf); -//simulate +//simulate specific void simulate(double delta_t); //Aux. methods @@ -77,7 +79,7 @@ class Puma560Sim : public RobotSim //Specific inverse methode of Puma 560 bool PUMA560inverseKinematics(Transformation3D t06, vector &_q, unsigned char _conf=0x00); - void goTo(vector _q); + //void goTo(vector _q); /*** Ranges coordiantes: @@ -91,15 +93,6 @@ Ranges coordiantes: private: CylindricalPart *light; - unsigned char conf; - vector q_init; - vector q_target; - - //cinematic simulation atributes - vector a0,a1,a2,a3; - double time; - double targetTime; - int f;//setFlash() }; diff --git a/src/simulation/quadrotorsim.cpp b/src/simulation/quadrotorsim.cpp index 6106d0f..fb39b59 100644 --- a/src/simulation/quadrotorsim.cpp +++ b/src/simulation/quadrotorsim.cpp @@ -26,6 +26,18 @@ void QuadrotorSim::readFromStream(Stream& stream) { SolidEntity::readFromStream(stream); } + +void QuadrotorSim::writeToXML(XMLElement* parent) +{ + + SolidEntity::writeToXML(parent); +} + +void QuadrotorSim::readFromXML(XMLElement* parent) +{ + SolidEntity::readFromXML(parent); +} + void QuadrotorSim::simulate(double t) { Transformation3D position=getAbsoluteT3D(); diff --git a/src/simulation/quadrotorsim.h b/src/simulation/quadrotorsim.h index 6516ac8..bc7a806 100644 --- a/src/simulation/quadrotorsim.h +++ b/src/simulation/quadrotorsim.h @@ -48,6 +48,8 @@ class QuadrotorSim : public Quadrotor, public ComposedEntity //Serializers virtual void writeToStream(Stream& stream); virtual void readFromStream(Stream& stream); + virtual void writeToXML(XMLElement* parent); + virtual void readFromXML(XMLElement* parent); //Constructor QuadrotorSim(double radius=0.25); diff --git a/src/simulation/wheeledbasesim.cpp b/src/simulation/wheeledbasesim.cpp index b160985..b64a8b4 100644 --- a/src/simulation/wheeledbasesim.cpp +++ b/src/simulation/wheeledbasesim.cpp @@ -42,6 +42,64 @@ void WheeledBaseSim::readFromStream(Stream& stream) wheels[2]=Vector3D(-large/2,(width+wheel_width)/2,wheel_radius); wheels[3]=Vector3D(-large/2,-(width+wheel_width)/2,wheel_radius); } + + +void WheeledBaseSim::writeToXML(XMLElement* parent) +{ + //SolidEntity::writeToStream(stream); + //stream<(width).c_str()); + //XMLVariable* larg= new XMLVariable ("large",aux.string_Convert(large).c_str()); + //XMLVariable* wheel_rad= new XMLVariable ("wheel_radius",aux.string_Convert(wheel_radius).c_str()); + //XMLVariable* wheel_wid= new XMLVariable ("wheel_width",aux.string_Convert(wheel_width).c_str()); + if (speed) + { + XMLVariable* _speed= new XMLVariable ("speed",XMLAux::string_Convert(speed).c_str()); + parent->AddVariable(_speed); + } + if (rotSpeed) + { + XMLVariable* _rotSpeed= new XMLVariable ("rotSpeed",XMLAux::string_Convert(rotSpeed).c_str()); + parent->AddVariable(_rotSpeed); + } + + + //wheeledBase->AddVariable(wid); + //wheeledBase->AddVariable(larg); + //wheeledBase->AddVariable(wheel_rad); + //wheeledBase->AddVariable(wheel_wid); + + + //parent->AddElement(wheeledBase); + + odometry.writeToXML(parent); + +} + +void WheeledBaseSim::readFromXML(XMLElement* parent) +{ + SolidEntity::readFromXML(parent); + odometry.readFromXML(parent); + + if (parent->FindVariableZ("speed")) + { + speed=XMLAux::GetValueDouble(parent->FindVariableZ("speed")); + + } + if (parent->FindVariableZ("rotSpeed")) + { + rotSpeed=XMLAux::GetValueDouble(parent->FindVariableZ("rotSpeed")); + + } + +} void WheeledBaseSim::simulate(double delta_t) { diff --git a/src/simulation/wheeledbasesim.h b/src/simulation/wheeledbasesim.h index 814b593..e5becdf 100644 --- a/src/simulation/wheeledbasesim.h +++ b/src/simulation/wheeledbasesim.h @@ -48,6 +48,8 @@ class WheeledBaseSim : public WheeledBase, public ComposedEntity //Serializers virtual void writeToStream(Stream& stream); virtual void readFromStream(Stream& stream); + virtual void writeToXML(XMLElement* parent); + virtual void readFromXML(XMLElement* parent); //Constructor WheeledBaseSim(double w=0.35, double l=0.5, double wh_radius=0.1, double wh_width=0.05); diff --git a/src/world/actuator.cpp b/src/world/actuator.cpp new file mode 100644 index 0000000..0488b4b --- /dev/null +++ b/src/world/actuator.cpp @@ -0,0 +1,326 @@ +/********************************************************************** + * + * This code is part of the MRcore project + * Author: Rodrigo Azofra Barrio &Miguel Hernando Gutierrez + * + * + * MRcore is licenced under the Common Creative License, + * Attribution-NonCommercial-ShareAlike 3.0 + * + * You are free: + * - to Share - to copy, distribute and transmit the work + * - to Remix - to adapt the work + * + * Under the following conditions: + * - Attribution. You must attribute the work in the manner specified + * by the author or licensor (but not in any way that suggests that + * they endorse you or your use of the work). + * - Noncommercial. You may not use this work for commercial purposes. + * - Share Alike. If you alter, transform, or build upon this work, + * you may distribute the resulting work only under the same or + * similar license to this one. + * + * Any of the above conditions can be waived if you get permission + * from the copyright holder. Nothing in this license impairs or + * restricts the author's moral rights. + * + * It is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. + **********************************************************************/ + + +#include "actuator.h" + +#include + +namespace mr{ +IMPLEMENT_MR_OBJECT(Actuator) + +Actuator::Actuator(double _speed,double _maxSpeed,double _acceleration, double _maxAcceleration) +{ + name="Actuator"; + s_Joint=0; + maxSpeed=_maxSpeed; + speed=_speed; + target=0; + targetActive=false; + acceleration=_acceleration; + maxAcceleration=_maxAcceleration; + + a0=a1=a2=a3=0.0; + + activateTVP=true;activateCPT=false; + typeTrajectoryTVP="MaximumSpeedAcceleration"; + + // + index=0; +} + + +Actuator::~Actuator(void) +{ + +} + +void Actuator::setSimulationParameters(double _maxSpeed, double _maxAcceleration) +{ + if(_maxSpeed<0) + return; + maxSpeed=_maxSpeed; + if(_maxAcceleration<=0) + return; + maxAcceleration=_maxAcceleration; +} + + +double Actuator::setSpeed(double sp) +{ + if(sp<0)return speed; + speed=sp>maxSpeed?maxSpeed:sp; + return speed; +} + +double Actuator::setAcceleration(double ac) +{ + if(ac<0)return acceleration; + acceleration=ac>maxAcceleration?maxAcceleration:ac; + return acceleration; +} + +bool Actuator::setTarget(double val) +{ + + if (s_Joint) + { + + double valMax,valMin; + s_Joint->getMaxMin(valMax,valMin); + + if((valvalMax)) + return false; + target=val; + targetActive=true; + return true; + } + else + return false; +} + +//bool Actuator::setTargetIntermediate(double _time) +//{ +// +// if (s_Joint) +// { +// double _targetInterm = a0 + a1*_time + a2*_time*_time + a3*_time*_time*_time; +// double valMax,valMin; +// s_Joint->getMaxMin(valMax,valMin); +// if((_targetIntermvalMax)) +// { +// targetActive=false; +// return false; +// } +// targetIntermediate=_targetInterm; +// targetActive=true; +// return true; +// } +// else +// return false; +//} + + + +void Actuator::simulate(double delta_t) +{ + if (s_Joint) + { + if(activateCPT) + simulateCPT(delta_t); + if(activateTVP) + simulateTVP(delta_t); + //simulateSPLINE(delta_t); + } + else + return; +} + + +void Actuator::linkTo (PositionableEntity *p) +{ + SimpleJoint* simpJoint = dynamic_cast (p); + + if (simpJoint) + { + s_Joint=simpJoint; + PositionableEntity::LinkTo(p); + } +} + + +/******************************************************** + METHODS TO CALCULATE TARGET BY CUBIC POLINOMIAL +*********************************************************/ +void Actuator::setCubicPolinomialCoeficients(double path,double targetTime) +{ + a0=s_Joint->getValue();//Current coordiantes + a1=0.0; + a2=( 3*(path)/(targetTime*targetTime)); + a3=(-2*(path)/(targetTime*targetTime*targetTime)); +} + +bool Actuator::setTargetCPT(double _time) +{ + if (_time<0) return false; + + double val=a0 + a1*_time + a2*_time*_time + a3*_time*_time*_time; + double sp=a1+ 2*a2*_time + 3*a3*_time*_time; + + setSpeed(sp); + + return setTarget(val); +} + +void Actuator::simulateCPT(double delta_t) +{ + if(targetActive==false)return; + + double value=s_Joint->getValue(); + double d=target-value; + double inc=delta_t*speed; + + if(d<0)inc=((-incd?d:inc); + if(dsetValue(target); + } + else s_Joint->setValue(value+inc); + +} + +/****************************************************************** + METHODS TO CALCULATE TARGET BY TRAPEZOIDAL VELOCITY PROFILE +*******************************************************************/ + +bool Actuator::setTargetTVP(double qInit,double q_target,int signMovement,double timeInit,double timeFinal,double ta, double _time) +{ + double val; + + if(getTypeTrajectoryTVP()=="MaximumSpeedAcceleration") + { + //Acceleration phase + if (_time<(timeInit+ta) && _time>=timeInit) + val=qInit+signMovement*((getAcceleration()*0.5)*square(_time-timeInit)); + + //Velocity constant phase + if (_time>=(timeInit+ta) && _time<=(timeFinal-ta)) + val=qInit+signMovement*(getSpeed()*(_time-timeInit-ta*0.5)); + + //Deceleration phase + if (_time>(timeFinal-ta) && _time<=timeFinal) + val=q_target-signMovement*((getAcceleration()*0.5)*square(timeFinal-_time)); + } + else if(getTypeTrajectoryTVP()=="BangBang") + { + //Acceleration phase + if (_time<(timeFinal*0.5) && _time>=timeInit) + val=qInit+signMovement*((getMaxAcceleration()*0.5)*square(_time-timeInit)); + + //Deceleration phase + if (_time>=(timeFinal*0.5) && _time<=timeFinal) + val=q_target-signMovement*((getMaxAcceleration()*0.5)*square(timeFinal-_time)); + } + + return setTarget(val); +} + + +bool Actuator::setTypeTrajectoryTVP(string _type) +{ + typeTrajectoryTVP=string(); + + if(_type=="BangBang") + { + typeTrajectoryTVP=_type; + return true; + } + if(_type=="MaximumSpeedAcceleration") + { + typeTrajectoryTVP=_type; + return true; + } + + return false; +} + +void Actuator::simulateTVP(double delta_t) +{ + double value=s_Joint->getValue(); + + if(targetActive==false)return; + double d=fabs(target-value); + if(dsetValue(target); +} + + + +/****************************************************************** + METHODS TO CALCULATE TARGET WITH SPLINE TRAJECTORY +*******************************************************************/ +void Actuator::setVelocIntermediates (vector veloc) +{ + double auxsp; + for(int i=0;i<(int)veloc.size();i++) + { + if(veloc[i]<0)auxsp=0.0; + else + auxsp=veloc[i]>maxSpeed?maxSpeed:veloc[i]; + velocInter.push_back(auxsp); + } +} + + +void Actuator::simulateSPLINE(double delta_t) +{ + double value=s_Joint->getValue(); + + if(targetActive==false)return; + double d=fabs(target-value); + if(dsetValue(target); + + +} + +bool Actuator::setTargetSPLINE(double _time) +{ + if (_time<0) return false; + + double val=a0 + a1*_time + a2*_time*_time + a3*_time*_time*_time; +// double sp=a1+ 2*a2*_time + 3*a3*_time*_time; + +// setSpeed(sp); + + return setTarget(val); + +} + + +void Actuator::setCubicPolinomialCoeficientsSPLINE(double strech,double Tk) +{ + if(index>=(int)velocInter.size())return; + a0=s_Joint->getValue();//Current coordiantes + a1=velocInter[index]; + a2=( 1/Tk)*((3*(strech)/(Tk))-2*velocInter[index]-velocInter[index+1]); + a3=(1/(Tk*Tk))*(((-2*(strech))/Tk)+velocInter[index]+velocInter[index+1]); + + index++; +} + +}//mr diff --git a/src/world/actuator.h b/src/world/actuator.h new file mode 100644 index 0000000..7c26a7c --- /dev/null +++ b/src/world/actuator.h @@ -0,0 +1,131 @@ +/********************************************************************** + * + * This code is part of the MRcore project + * Author: Rodrigo Azofra Barrio & Miguel Hernando Gutierrez + * + * + * MRcore is licenced under the Common Creative License, + * Attribution-NonCommercial-ShareAlike 3.0 + * + * You are free: + * - to Share - to copy, distribute and transmit the work + * - to Remix - to adapt the work + * + * Under the following conditions: + * - Attribution. You must attribute the work in the manner specified + * by the author or licensor (but not in any way that suggests that + * they endorse you or your use of the work). + * - Noncommercial. You may not use this work for commercial purposes. + * - Share Alike. If you alter, transform, or build upon this work, + * you may distribute the resulting work only under the same or + * similar license to this one. + * + * Any of the above conditions can be waived if you get permission + * from the copyright holder. Nothing in this license impairs or + * restricts the author's moral rights. + * + * It is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. + **********************************************************************/ + +#ifndef __MRCORE__ACTUATOR__H +#define __MRCORE__ACTUATOR__H + + +#include "simplejoint.h" +#include "math/mrmath.h" +#include + +using namespace std; + +namespace mr +{ + class SimpleJoint; +/** + \class Actuator + Actuator -> Implementation to simulate simplejoints movement +*/ + +class Actuator: public PositionableEntity +{ + DECLARE_MR_OBJECT(Actuator) + //friend class World; + +protected: + SimpleJoint* s_Joint; + + //kinematic simulation atributes + double speed, maxSpeed; // m/s rad/s + double acceleration,maxAcceleration; //rad/sec^2 + + double target, targetIntermediate; + bool targetActive; //true if target have to be reached + string typeTrajectoryTVP; + //spline algorithm + double a0,a1,a2,a3; //polinomial coeficients + + bool activateTVP,activateCPT; + + int index; + vector velocInter; + + +public: +//constructors + +//Basic Constructor + Actuator(double _speed=1,double _maxSpeed=1,double _acceleration=0.1, double _maxAcceleration=0.5); + //Destructor + virtual ~Actuator(void); + + +//simulation methods + void setSimulationParameters(double _maxSpeed, double _maxAcceleration=PI/6);//max acelerate default=30º/sec^2 + + double setSpeed(double sp); + double setAcceleration(double ac); + + double getSpeed(){return speed;} + double getMaxSpeed(){return maxSpeed;} + double getAcceleration(){return acceleration;} + double getMaxAcceleration(){return maxAcceleration;} + + bool isMoving(){return targetActive;} + void linkTo (PositionableEntity *p); + + virtual void simulate(double delta_t); + + void activateCubicPolynomialTrajectory(){activateTVP=false;activateCPT=true;} + void activateTrapezoidalVelocityTrajectory(){activateTVP=true;activateCPT=false;} + + bool setTarget(double val); + double getTarget(){return target;} + +//Cubic Polinomial Trajectory (CPT) + + void simulateCPT(double delta_t); + bool setTargetCPT(double _time); + void setCubicPolinomialCoeficients(double path,double targetTime); + + +//Trapezoidal Velocity Profile tarjectory (TVP) + + void simulateTVP(double delta_t); + bool setTargetTVP(double qInit,double q_target,int signMovement,double timeInit,double timeFinal,double ta, double _time); + bool setTypeTrajectoryTVP(string _type); + string getTypeTrajectoryTVP(){return typeTrajectoryTVP;} + +// Spline trajetory (interpolation points) + void setVelocIntermediates (vector veloc); + void simulateSPLINE(double delta_t); + bool setTargetSPLINE(double _time); + void setCubicPolinomialCoeficientsSPLINE(double strech,double Tk); + + + +}; + +}; +#endif //__MRCORE__ACTUATOR__H diff --git a/src/world/composedentity.cpp b/src/world/composedentity.cpp index e0e4468..23b1d67 100644 --- a/src/world/composedentity.cpp +++ b/src/world/composedentity.cpp @@ -50,6 +50,19 @@ void ComposedEntity::readFromStream(Stream& stream) } +void ComposedEntity::writeToXML(XMLElement* parent) +{ + //XMLElement* compos=new XMLElement(parent,"composedentity"); + //parent->AddElement(compos); + SolidEntity::writeToXML(parent); + EntitySet::writeToXML(parent); +} +void ComposedEntity::readFromXML(XMLElement* parent) +{ + SolidEntity::readFromXML(parent); + EntitySet::readFromXML(parent); +} + ComposedEntity::ComposedEntity(void) { @@ -136,9 +149,9 @@ bool ComposedEntity::checkCollisionWith(SolidEntity &solid) } return false; } -void ComposedEntity::simulate(double t) +void ComposedEntity::simulate(double delta_t) { for(int i=0;i<(int)(objects.size());i++) - objects[i]->simulate(t); + objects[i]->simulate(delta_t); } }//mr \ No newline at end of file diff --git a/src/world/composedentity.h b/src/world/composedentity.h index f909b41..c586d63 100644 --- a/src/world/composedentity.h +++ b/src/world/composedentity.h @@ -81,6 +81,9 @@ class ComposedEntity: public EntitySet, public SolidEntity //serialization virtual void writeToStream(Stream& stream); virtual void readFromStream(Stream& stream); + virtual void writeToXML(XMLElement* parent); + virtual void readFromXML(XMLElement* parent); + //Draw the composed object void drawGL(); //returns the bounding box... @@ -99,7 +102,7 @@ class ComposedEntity: public EntitySet, public SolidEntity //the descomposition in parts virtual bool checkCollisionWith(SolidEntity &solid); //redefinition of simulate (positionable Entity) - virtual void simulate(double t); + virtual void simulate(double delta_t); protected: bool linkToBase(PositionableEntity *p); }; diff --git a/src/world/cylindricalpart.cpp b/src/world/cylindricalpart.cpp index 571de8c..e165f0f 100644 --- a/src/world/cylindricalpart.cpp +++ b/src/world/cylindricalpart.cpp @@ -45,6 +45,40 @@ void CylindricalPart::readFromStream(Stream& stream) { PrismaticPart::readFromStream(stream); } + +void CylindricalPart::writeToXML(XMLElement* parent) +{ + //PrismaticPart::writeToXML(parent); + + if (height!=1.00) + { + XMLVariable* h=new XMLVariable("height",XMLAux::string_Convert(height).c_str()); + parent->AddVariable(h); + } + if (radius!=1.00) + { + XMLVariable* r=new XMLVariable("radius",XMLAux::string_Convert(radius).c_str()); + parent->AddVariable(r); + } + PrimitiveSolidEntity::writeToXML(parent); +} +void CylindricalPart::readFromXML(XMLElement* parent) +{ + double h=height,rad=radius; + + if (parent->FindVariableZ("height")) + { + h=XMLAux::GetValueDouble(parent->FindVariableZ("height")); + } + if (parent->FindVariableZ("radius")) + { + rad=XMLAux::GetValueDouble(parent->FindVariableZ("radius")); + } + + setHeightAndRadius(h,rad); + PrimitiveSolidEntity::readFromXML(parent); +} + ostream& operator<<(ostream& os, const CylindricalPart& p) { //os< pObj=file.getXMLElementsObjects(); + + //compruebo si su parent es otro objeto de la colección + for(i=0;igetLinkedTo()) + { + if(this==objects[i]->getLinkedTo()->getOwner()) + { + int a=getIndexOf(objects[i]->getLinkedTo()); + + string _name=XMLAux::GetValueCadena(pObj[a]->FindVariableZ("name",true)); + XMLVariable* linkedTo; + bool repeatedOrEmptyName=false; + //linked one object with another one (i,a) + if (_name.empty()) + repeatedOrEmptyName=true; + + else + { + for (int j=0;jFindVariableZ("name")) && a!=j) + repeatedOrEmptyName=true; + + } + } + + if (repeatedOrEmptyName) + linkedTo= new XMLVariable ("linkTo",XMLAux::setLinkTo("noName",a).c_str()); + + else + linkedTo= new XMLVariable ("linkTo",XMLAux::setLinkTo(_name,a).c_str()); + + + pObj[i]->AddVariable(linkedTo); + + } + } + } + + //write unions to tcp's + for(i=0;igetLinkedTo()) + { + Tcp *check=dynamic_cast(objects[i]->getLinkedTo()); + if(check){ + ComposedEntity *aux=dynamic_cast(check->getOwner()); + if(aux){ + if(this==aux->getOwner()){ + + int a=getIndexOf(aux); + int b=aux->getTcpIndex(check); + + + string _name=XMLAux::GetValueCadena(pObj[a]->FindVariableZ("name",true)); + string _nameTcp=string(); + XMLVariable* linkedToTcp; + bool repeatedName=false; + if(_name.empty()) + { + stringstream str; + str<<"Composed Entity"<FindVariableZ("name")->SetValue(_name.c_str()); + } + else + { + for (int j=0;jFindVariableZ("name")) && a!=j) + repeatedName=true; + + } + } + if (XMLAux::GetNameElement(pObj[a])=="ComposedEntity") + { + _nameTcp=XMLAux::GetValueCadena(pObj[a]->FindElementZ("Tcp")->FindVariableZ("name",true)); + if(_nameTcp.empty()) + { + stringstream str; + str<<"tcp"<FindElementZ("Tcp")->FindVariableZ("name")->SetValue(_nameTcp.c_str()); + } + if (repeatedName) + linkedToTcp= new XMLVariable ("linkToTcp",XMLAux::setLinkToTcp("noName",_nameTcp,b,a).c_str()); + else + linkedToTcp= new XMLVariable ("linkToTcp",XMLAux::setLinkToTcp(_name,_nameTcp,b,a).c_str()); + } + else + { + if(repeatedName) + linkedToTcp= new XMLVariable ("linkToTcp",XMLAux::setLinkToTcpDefault("noName",b,a).c_str()); + else + linkedToTcp= new XMLVariable ("linkToTcp",XMLAux::setLinkToTcpDefault(_name,b,a).c_str()); + + } + + pObj[i]->AddVariable(linkedToTcp); + + } + } + } + + } + } + +} + +void EntitySet::readFromXML(XMLElement* parent) +{ + int i,num=parent->GetChildrenNum(); + XMLElement** pObj=parent->GetChildren(); + XMLfile file(parent); + objects.clear(); + for(int i=0;i(file.read(pObj[i])); + addObject(aux); + } + + + for(i=0;iFindVariableZ("linkTo")) + { + XMLVariable* link=pObj[i]->FindVariableZ("linkTo"); + string type=XMLAux::getTypeConectionLink(link,false); + if (type=="conectionId") + { + int id_linked=link->GetValueInt(); + for (int j=0;jFindVariableZ("id")) + { + if (id_linked==pObj[j]->FindVariableZ("id")->GetValueInt()) + objects[i]->LinkTo(objects[j]); + } + } + } + else if(type=="conectionNames") + { + string nameLink=XMLAux::GetNameLinkTo(link); + for (int j=0;jFindVariableZ("name"))) + objects[i]->LinkTo(objects[j]); + } + + } + } + } + + for(i=0;iFindVariableZ("linkToTcp")) + { + XMLVariable* linkTcp=pObj[i]->FindVariableZ("linkToTcp"); + string type=XMLAux::getTypeConectionLink(linkTcp); + + if (type=="conectionId") + { + int ids[2]={0}; + XMLAux::GetValueOwnerAndTcp(linkTcp,ids); + int id_OwnerTcp=ids[0], id_linkTcp=ids[1]; + + if(id_OwnerTcp>=0) + { + for (int j=0;jFindVariableZ("id")) + { + if (id_OwnerTcp==pObj[j]->FindVariableZ("id")->GetValueInt()) + { + ComposedEntity *aux=dynamic_cast(objects[j]); + + if(aux) + { + if (XMLAux::GetNameElement(pObj[j])=="ComposedEntity") + { + int numChildCompos=pObj[j]->GetChildrenNum(); + if (numChildCompos) + { + XMLElement** childsComposed=pObj[j]->GetChildren(); + int indexTcp=0; + for (int z=0;zFindVariableZ("id",true)->GetValueInt()) + objects[i]->LinkTo(aux->getTcp(indexTcp)); + else if (indexTcp==id_linkTcp) + objects[i]->LinkTo(aux->getTcp(indexTcp)); + + indexTcp++; + + } + } + } + } + else + objects[i]->LinkTo(aux->getTcp(id_linkTcp));//it's supposed that user knows id_linkTcp=Tcp's index + + } + } + } + } + } + } + else if (type=="conectionNames") + { + vector names=XMLAux::GetNameOwnerAndTcp(linkTcp); + int idTcp=XMLAux::getIndTcp(names[1]); + string nameOnlyTcp=XMLAux::getOnlyNameTcp(names[1]); + for (int j=0;jFindVariableZ("name"))) + { + ComposedEntity *aux=dynamic_cast(objects[j]); + + if(aux) + { + + if (XMLAux::GetNameElement(pObj[j])=="ComposedEntity") + { + int numChildCompos=pObj[j]->GetChildrenNum(); + if(numChildCompos) + { + XMLElement** childsComposed=pObj[j]->GetChildren(); + int indexTcp=0; + + for (int z=0;zFindVariableZ("name")) && + idTcp==childsComposed[z]->FindVariableZ("id")->GetValueInt()) + objects[i]->LinkTo(aux->getTcp(indexTcp)); + else if (nameOnlyTcp==XMLAux::GetValueCadena(childsComposed[z]->FindVariableZ("name")) && + idTcp==indexTcp) + objects[i]->LinkTo(aux->getTcp(indexTcp)); + else if(idTcp==childsComposed[z]->FindVariableZ("id")->GetValueInt()) + objects[i]->LinkTo(aux->getTcp(indexTcp)); + else if (idTcp==indexTcp) + objects[i]->LinkTo(aux->getTcp(indexTcp)); + + indexTcp++; + } + } + } + } + else + objects[i]->LinkTo(aux->getTcp(idTcp)); + + } + } + } + + } + } + } +} EntitySet::EntitySet(void) { diff --git a/src/world/entityset.h b/src/world/entityset.h index 50f6326..491c151 100644 --- a/src/world/entityset.h +++ b/src/world/entityset.h @@ -78,6 +78,9 @@ class EntitySet:virtual public GLObject //serialization virtual void writeToStream(Stream& stream); virtual void readFromStream(Stream& stream); + virtual void writeToXML(XMLElement* parent); + virtual void readFromXML(XMLElement* parent); + //number of objects included int getNumObjects(){return (int)objects.size();} //method for adding an object to the EntitySet diff --git a/src/world/facesetpart.cpp b/src/world/facesetpart.cpp index cd67a48..5a6128e 100644 --- a/src/world/facesetpart.cpp +++ b/src/world/facesetpart.cpp @@ -59,6 +59,32 @@ void FaceSetPart::readFromStream(Stream& stream) } } + +void FaceSetPart::writeToXML(XMLElement* parent) +{ + SolidEntity::writeToXML(parent); + int num=(int)faces.size(); + for(int i=0;iGetChildrenNum(); + if (num) + { + XMLElement** _faces=parent->GetChildren(); + for(int i=0;i *ipoints); diff --git a/src/world/joint.h b/src/world/joint.h index 48051e9..863ff38 100644 --- a/src/world/joint.h +++ b/src/world/joint.h @@ -68,8 +68,11 @@ class Joint: public PositionableEntity //methods //serialization - virtual void writeToStream(Stream& stream){PositionableEntity::writeToStream(stream);}; - virtual void readFromStream(Stream& stream){PositionableEntity::readFromStream(stream);}; + virtual void writeToStream(Stream& stream){PositionableEntity::writeToStream(stream);} + virtual void readFromStream(Stream& stream){PositionableEntity::readFromStream(stream);} + virtual void writeToXML(XMLElement* parent){PositionableEntity::writeToXML(parent);} + virtual void readFromXML(XMLElement* parent){PositionableEntity::readFromXML(parent);} + //returns the location that would be used for objects referred to this positionable object //this method have to be overwritten by joints and tcp´s virtual ReferenceSystem *getReferenciableLocation(){return &outputLocation;} diff --git a/src/world/material.cpp b/src/world/material.cpp index 431be3f..61b0f04 100644 --- a/src/world/material.cpp +++ b/src/world/material.cpp @@ -51,4 +51,58 @@ void Material::readFromStream(Stream& stream) { stream>>color.r>>color.g>>color.b; } + +void Material::writeToXML(XMLElement* parent) +{ + //XMLAux aux; + bool defaultR=true, defaultG=true, defaultB=true; + + XMLElement* colour=new XMLElement(parent,"colour"); + + if (color.r!=0.5) + { + XMLVariable* r= new XMLVariable("r",XMLAux::string_Convert(color.r).c_str()); + colour->AddVariable(r); + defaultR=false; + } + if (color.g!=0.5) + { + XMLVariable* g= new XMLVariable("g",XMLAux::string_Convert(color.g).c_str()); + colour->AddVariable(g); + defaultG=false; + } + if (color.b!=0.5) + { + XMLVariable* b= new XMLVariable("b",XMLAux::string_Convert(color.b).c_str()); + colour->AddVariable(b); + defaultB=false; + } + + if (defaultR && defaultG && defaultB) + return; + else + parent->AddElement(colour); + +} + +void Material::readFromXML(XMLElement* parent) +{ + XMLElement* colour; + double r=color.r,g=color.g,b=color.b; + + if(parent->FindElementZ("colour")) + { + colour=parent->FindElementZ("colour"); + + if(colour->FindVariableZ("r")) + r=XMLAux::GetValueDouble(colour->FindVariableZ("r")); + if(colour->FindVariableZ("g")) + g=XMLAux::GetValueDouble(colour->FindVariableZ("g")); + if(colour->FindVariableZ("b")) + b=XMLAux::GetValueDouble(colour->FindVariableZ("b")); + } + + setColor(r,g,b); + +} }//mr diff --git a/src/world/material.h b/src/world/material.h index 9401720..81bf17e 100644 --- a/src/world/material.h +++ b/src/world/material.h @@ -36,6 +36,8 @@ #include "../base/object.h" +#include "../base/xmlaux.h" +#include "../base/xml.h" namespace mr{ //currently a very simplified version. In future it will be posible to @@ -59,6 +61,8 @@ struct Color{ } void writeToStream(Stream& stream); void readFromStream(Stream& stream); + void writeToXML(XMLElement* parent); + void readFromXML(XMLElement* parent); void loadMaterial(); }; diff --git a/src/world/meshpart.cpp b/src/world/meshpart.cpp index 71e1dc6..626e22b 100644 --- a/src/world/meshpart.cpp +++ b/src/world/meshpart.cpp @@ -53,6 +53,23 @@ void MeshPart::readFromStream(Stream& stream) createWiredModel(); } +void MeshPart::writeToXML(XMLElement* parent) +{ + PrimitiveSolidEntity::writeToXML(parent); + mesh.writeToXML(parent); + +} + +void MeshPart::readFromXML(XMLElement* parent) +{ + PrimitiveSolidEntity::readFromXML(parent); + mesh.readFromXML(parent); + //actualizaciones + absoluteMesh=mesh; + setMeshNeedToBeUpdated(); + createWiredModel(); +} + void MeshPart::createWiredModel() { int i,num; diff --git a/src/world/meshpart.h b/src/world/meshpart.h index 544ddb2..248d90a 100644 --- a/src/world/meshpart.h +++ b/src/world/meshpart.h @@ -93,8 +93,14 @@ class MeshPart: public PrimitiveSolidEntity void setMeshNeedToBeUpdated(){meshNeedToBeUpdated=true;} void updateMesh(); void addTriangle(Vector3D a,Vector3D b, Vector3D c); +//serialization virtual void writeToStream(Stream& stream); virtual void readFromStream(Stream& stream); + virtual void writeToXML(XMLElement* parent); + virtual void readFromXML(XMLElement* parent); + + + void drawGL(); //specific operations that have to be implemented due to its solid entity condition bool segmentIntersection(const Segment3D &s,vector *ipoints); diff --git a/src/world/positionableentity.cpp b/src/world/positionableentity.cpp index 5030a30..b2b4976 100644 --- a/src/world/positionableentity.cpp +++ b/src/world/positionableentity.cpp @@ -35,6 +35,7 @@ #include "world.h" #include "composedentity.h" #include "gl/gltools.h" + #include namespace mr{ @@ -52,6 +53,37 @@ void PositionableEntity::readFromStream(Stream& stream) setRelativeT3D(aux); } +void PositionableEntity::writeToXML(XMLElement* parent) +{ + + XMLVariable* nam= new XMLVariable("name",name.c_str()); + parent->AddVariable(nam); + + getRelativeT3D().writeToXML(parent); + + +} + +void PositionableEntity::readFromXML(XMLElement* parent) +{ + //XMLElement* posi; + //if(parent->FindElementZ("positionableEntity")) + // posi=parent->FindElementZ("positionableEntity"); + //else + // posi=parent; + + char cad[50]={0}; + if(parent->FindVariableZ("name")) + { + name=XMLAux::GetValueCadena(parent->FindVariableZ("name")); + } + + Transformation3D aux; + aux.readFromXML(parent); + setRelativeT3D(aux); +} + + ostream& operator<<(ostream& os, const PositionableEntity& p) { //os< #include "../base/globject.h" #include "referencesystem.h" +#include "../base/xmlaux.h" +#include "../base/xml.h" @@ -103,13 +105,15 @@ class PositionableEntity: virtual public GLObject //serialization virtual void writeToStream(Stream& stream); virtual void readFromStream(Stream& stream); + virtual void writeToXML(XMLElement* parent); + virtual void readFromXML(XMLElement* parent); //Draw positional entity void drawGL(); //virtual function for time simulation. time inteval in seconds virtual void simulate(double delta_t){}//do nothing //Associate an entity with other - void LinkTo(PositionableEntity *pe); + virtual void LinkTo(PositionableEntity *pe); //returns the location that would be used for objects referred to this positionable object //this method have to be overwritten by joints and tcp´s virtual ReferenceSystem *getReferenciableLocation(); diff --git a/src/world/primitivesolidentity.h b/src/world/primitivesolidentity.h index 16c7357..10e1391 100644 --- a/src/world/primitivesolidentity.h +++ b/src/world/primitivesolidentity.h @@ -82,6 +82,8 @@ class PrimitiveSolidEntity: public SolidEntity //serialization virtual void writeToStream(Stream& stream){SolidEntity::writeToStream(stream);}; virtual void readFromStream(Stream& stream){SolidEntity::readFromStream(stream);}; + virtual void writeToXML(XMLElement* parent){SolidEntity::writeToXML(parent);} + virtual void readFromXML(XMLElement* parent){SolidEntity::readFromXML(parent);} void locationUpdated(); diff --git a/src/world/prismaticpart.cpp b/src/world/prismaticpart.cpp index e7f6984..6c2aa1d 100644 --- a/src/world/prismaticpart.cpp +++ b/src/world/prismaticpart.cpp @@ -53,6 +53,36 @@ void PrismaticPart::readFromStream(Stream& stream) f.readFromStream(stream); setPolygonalBase(f); //this function implyies the updating of all aux models } + +void PrismaticPart::writeToXML(XMLElement* parent) +{ + if (height!=1.0) //it is its default value + { + //XMLAux aux; + XMLVariable* alt= new XMLVariable("height",XMLAux::string_Convert(height).c_str()); + parent->AddVariable(alt); + } + + PrimitiveSolidEntity::writeToXML(parent); + polygonalBase.writeToXML(parent); + +} + +void PrismaticPart::readFromXML(XMLElement* parent) +{ + double h=height; + + if(parent->FindVariableZ("height")) + h=XMLAux::GetValueDouble(parent->FindVariableZ("height")); + + setHeight(h); + Face f; + f.readFromXML(parent); + setPolygonalBase(f); + PrimitiveSolidEntity::readFromXML(parent); + +} + ostream& operator<<(ostream& os, const PrismaticPart& p) { //os< &list_of_vertex); void setRegularPolygonBase(double radius,int numVertex); void setHeight(double _height); +//serialization virtual void writeToStream(Stream& stream); virtual void readFromStream(Stream& stream); + virtual void writeToXML(XMLElement* parent); + virtual void readFromXML(XMLElement* parent); + void drawGL(); double getHeight(){return height;} Face getPolygonalBase(); diff --git a/src/world/robotsim.cpp b/src/world/robotsim.cpp index 40746fe..49eacf3 100644 --- a/src/world/robotsim.cpp +++ b/src/world/robotsim.cpp @@ -153,10 +153,11 @@ the T3D is a relative transformation to the base (link[0]) } bool RobotSim::moveTo(const vector & _q) { - int i; + if(_q.size()!=joints.size())return false; if(!checkJointValues(_q))return false; //limits checking - for(i=0;i<(int)joints.size();i++)joints[i]->setValue(_q[i]); + //for(int i=0;i<(int)joints.size();i++)joints[i]->setValue(_q[i]); + goTo(_q); return true; } bool RobotSim::moveToAbs(Transformation3D t3d, unsigned char conf) @@ -185,9 +186,429 @@ bool RobotSim::moveTo(double *_q) for(int i=0;igetValue());//* + + if( (time+delta_t) >= targetTime)//* + { + for(int i=0;i<(int)joints.size();i++) + actuators[i]->setTarget(q_target[i]); + + q_target.clear(); + time=0.0; + targetTime=0.0; + posInit=true; + qInit.clear(); + } + else + { + checkActivationTrajectory(); + + if (activateTVP) + calculateTargetTVP(time); + else if (activateCPT) + calculateTargetCPT(time); + //calculateTargetSPLINE(0.1); + + time+=delta_t; + } + } + ComposedEntity::simulate(delta_t); } + + +void RobotSim::goTo(vector q) +{ + + q_target=q;//Target loaded + + double error=0; + for(int i=0;i<(int)joints.size();i++) + error+=fabs(q_target[i] - joints[i]->getValue());//* + + if(fabs(error) < EPS) + { + q_target.clear(); + return; + } + else + { + time=0.0; + + checkActivationTrajectory(); + + if (activateTVP) + calculateTargetTimeTVP(); + else if (activateCPT) + calculateTargetTimeCPT(); + //calculateTargetTimeSPLINE(); + } + + return; +} + +void RobotSim::checkActivationTrajectory() +{ + //Check if there are two trajectories activated/desactivated + if((!activateTVP && !activateCPT) || (activateTVP && activateCPT)) + {//default selection TVP + activateTVP=true; + activateCPT=false; + } +} + +void RobotSim::goToAbs(Transformation3D t) +{ + vector q; + if(!inverseKinematicsAbs(t,q))return; + goTo(q); + return; +} + +void RobotSim::goTo(Transformation3D t) +{ + vector q; + unsigned char conf=getCurrentConfiguration(); + if(!inverseKinematics(t,q,conf))return; + goTo(q); + return; +} + +bool RobotSim::checkActuatorsIsMoving() +{ + for(int i=0;i<(int)actuators.size();i++) + { + if(actuators[i]->isMoving()) + return true; + } + return false; +} + +/************************************************************* + METHODS TO CALCULATE TARGET BY CUBIC POLINOMIAL TRAJECTORY +**************************************************************/ +void RobotSim::calculateTargetTimeCPT() +{ + double lowestSpeed=100.0; + double longestPath=0.0; + vector pathJoint; + + + for(int i=0;i<(int)actuators.size();i++) + { + pathJoint.push_back(q_target[i] - joints[i]->getValue());//Path each coordinate: target minus current coordinates + + if(actuators[i]->getMaxSpeed() <= lowestSpeed) + lowestSpeed = actuators[i]->getMaxSpeed(); + if(fabs(pathJoint[i])>= longestPath) + longestPath =fabs(pathJoint[i]);//* + } + targetTime = longestPath / lowestSpeed; + + for(int i=0;i<(int)actuators.size();i++) + { + actuators[i]->setCubicPolinomialCoeficients(pathJoint[i],targetTime); + } +} + +void RobotSim::calculateTargetCPT(double _time) +{ + for(int i=0;i<(int)actuators.size();i++) + { + actuators[i]->setTargetCPT(_time); + } +} + +/****************************************************************** + METHODS TO CALCULATE TARGET BY TRAPEZOIDAL VELOCITY PROFILE +*******************************************************************/ + +void RobotSim::calculateTargetTimeTVP() +{ + if (joints.size()!=actuators.size())return; + if (q_target.size()!=joints.size())return; + + posInit=true; + + double auxTargetTime,auxTa; + double maxTa,maxTargetTime=0.0; + double maxSpeed,maxAcceleration; + double condition; + vector path; + int index=0; + + /* Check which is the maximum time to get the target by all the joints. + For this we obtain the max values of the speed and acceleration of the joints and check + with their own path to travel */ + + for(int i=0;i<(int)actuators.size();i++) + { + maxSpeed=actuators[i]->getMaxSpeed(); + maxAcceleration=actuators[i]->getMaxAcceleration(); + + condition=(maxSpeed*maxSpeed)/maxAcceleration; + + path.push_back(fabs(joints[i]->getValue()-q_target[i])); + + if (condition>path[i])//Bang Bang movement + { + auxTargetTime=2*sqrt(path[i]/maxAcceleration);//targetTime + auxTa=0.5*auxTargetTime;//ta + } + else // speed and acceleration with maximus values + { + auxTargetTime=path[i]/maxSpeed + maxSpeed/maxAcceleration;//targetTime + auxTa=maxSpeed/maxAcceleration;//ta + } + + if (maxTargetTimesetTypeTrajectoryTVP("BangBang"); + else + { + actuators[index]->setSpeed(actuators[index]->getMaxSpeed()); + actuators[index]->setAcceleration(actuators[index]->getMaxAcceleration()); + actuators[index]->setTypeTrajectoryTVP("MaximumSpeedAcceleration"); + } + + adjustMotionJointsTVP(path,maxTargetTime,maxTa,index); +} + +void RobotSim::adjustMotionJointsTVP (vector _path,double _targetTime,double _ta,int index) +{ + //we adjust the speed and acceleration to get all the joints are finishing the movement at the same time + + if(_targetTime<=0 && _ta<=0)return; + + double speed,acel,maxSp,maxAcel; + vector path=_path; + + for(int i=0;i<(int)actuators.size();i++) + { + if(i==index)continue; + maxSp=actuators[i]->getMaxSpeed(); + maxAcel=actuators[i]->getMaxAcceleration(); + + //movement with speed and acceleration values adjust to timeTarget global + speed=path[i]/(_targetTime-_ta); + acel=speed/_ta; + + if (speed>maxSp || acel>maxAcel)//if true, try to change the movement to Bang Bang + { + acel=path[i]/(_ta*_ta); + speed=acel*_ta; + + if (speed>maxSp || acel>maxAcel) + return; //we have to recalculate the ta time to this actuator + actuators[i]->setAcceleration(acel); + actuators[i]->setSpeed(speed); + actuators[i]->setTypeTrajectoryTVP("BangBang"); + } + else + { + actuators[i]->setSpeed(speed); + actuators[i]->setAcceleration(acel); + actuators[i]->setTypeTrajectoryTVP("MaximumSpeedAcceleration"); + } + } +} + + +void RobotSim::calculateTargetTVP(double _time) +{ + + double timeInit,timeFinal; + //double target; + timeInit=0.0; + timeFinal=targetTime; + int signMovement=1; + + if(q_target.size()!=actuators.size())return; + if(joints.size()!=actuators.size())return; + + if (posInit) //we need the initial values of the joints + { + for (int i=0;igetValue()); + + } + posInit=false; + } + + for (int i=0;isetTargetTVP(qInit[i],q_target[i],signMovement,timeInit,timeFinal,ta,_time); + + //if(actuators[i]->getTypeTrajectoryTVP()=="MaximumSpeedAcceleration") + //{ + // //Acceleration phase + // if (_time<(timeInit+ta) && _time>=timeInit) + // target=qInit[i]+signMovement*((actuators[i]->getAcceleration()*0.5)*square(_time-timeInit)); + + // //Velocity constant phase + // if (_time>=(timeInit+ta) && _time<=(timeFinal-ta)) + // target=qInit[i]+signMovement*(actuators[i]->getSpeed()*(_time-timeInit-ta*0.5)); + + // //Deceleration phase + // if (_time>(timeFinal-ta) && _time<=timeFinal) + // target=q_target[i]-signMovement*((actuators[i]->getAcceleration()*0.5)*square(timeFinal-_time)); + //} + //else if(actuators[i]->getTypeTrajectoryTVP()=="BangBang") + //{ + // //Acceleration phase + // if (_time<(timeFinal*0.5) && _time>=timeInit) + // target=qInit[i]+signMovement*((actuators[i]->getMaxAcceleration()*0.5)*square(_time-timeInit)); + + // //Deceleration phase + // if (_time>=(timeFinal*0.5) && _time<=timeFinal) + // target=q_target[i]-signMovement*((actuators[i]->getMaxAcceleration()*0.5)*square(timeFinal-_time)); + //} + + //actuators[i]->setTarget(target); + } + +} + + +/****************************************************************** + METHODS TO CALCULATE TARGET BY SPLINE TRAJECTORY +*******************************************************************/ + +void RobotSim::calculateTargetTimeSPLINE() +{ + double lowestSpeed=100.0; + double longestPath=0.0; + vector pathJoint; + + + for(int i=0;i<(int)actuators.size();i++) + { + pathJoint.push_back(q_target[i] - joints[i]->getValue());//Path each coordinate: target minus current coordinates + + if(actuators[i]->getMaxSpeed() <= lowestSpeed) + lowestSpeed = actuators[i]->getMaxSpeed(); + if(fabs(pathJoint[i])>= longestPath) + longestPath =fabs(pathJoint[i]);//* + } + targetTime = longestPath / lowestSpeed; + + double Tk=0.1,stretch=0.1; + int nIterations=(int)(targetTime/Tk); + vector a,b,c,d; + for (int i=0;i veloc= TDMA(a,b,c,d,nIterations); + for (int i=1;i<(int)actuators.size();i++) + { + actuators[i]->setVelocIntermediates(veloc); + } +} + +void RobotSim::calculateTargetSPLINE(double _time) +{ + double strech=0.1,Tk=0.1; + for(int i=0;i<(int)actuators.size();i++) + { + actuators[i]->setCubicPolinomialCoeficientsSPLINE(strech,Tk); + actuators[i]->setTargetSPLINE(_time); + } +} + + +vector RobotSim::TDMA (vector a, vector b, vector c,vector d, int nIterations) +{ + /*It's the Thomas algorithm to resolve tridiagonal matrix. It's useful + to calculate the intermediate values of the velocity in the points to + interpolate (vk). + + c'1=c1/b1 + c'i= ci/(bi-c'[i-1]*ai) i=2...n-1 + + d'1=d1/b1 + d'i=(di-d'[i-1]*ai)/(bi-c'[i-1]*ai) i=2...n + + xn=d'n + xi=d'i-c'i*x[i+1] i=n-1....1 + */ + + + + double denom; + vector gamma,betta,x; + gamma.push_back(0); + betta.push_back(0); + + int i=0; + + //a=2*(Tk+Tk)=4*Tk; b=Tk; c=Tk; + //dk = (3/(Tk*Tk))*((Tk*Tk*(path))+(Tk*Tk*(path))); + //dk = (3*(2*path)) + + //dk = 6*path; //more simplificate + + for (i=1;i<=nIterations;i++) + { + denom=a[i]*gamma[i-1]+b[i]; + if(i!=nIterations) + gamma.push_back((-c[i])/denom); + betta.push_back((d[i]-(a[i]*betta[i-1]))/denom); + } + + //calculate the solutions + x.push_back(betta[nIterations]); + int index=0; + + for (i=nIterations-1;i>=1;i--) + { + x.push_back(gamma[i]*x[index++]+betta[i]); + } + + //reordenate the vector to return all the velocities in order + vector veloc; + + veloc.push_back(0.0);//veloc init + + for(i=0;i<(int)x.size();i++) + { + veloc.push_back(x[i]); + } + veloc.push_back(0.0);//veloc final + + return veloc; + +} + + + };//Namespace mr \ No newline at end of file diff --git a/src/world/robotsim.h b/src/world/robotsim.h index 147df69..839f1b7 100644 --- a/src/world/robotsim.h +++ b/src/world/robotsim.h @@ -37,7 +37,7 @@ #include "simplejoint.h" #include "cylindricalpart.h" #include "tcp.h" - +#include "actuator.h" namespace mr { /*! @@ -61,6 +61,9 @@ class RobotSim : public ComposedEntity //Serializers virtual void writeToStream(Stream& stream){} virtual void readFromStream(Stream& stream){} + virtual void writeToXML(XMLElement* parent){} + virtual void readFromXML(XMLElement* parent){} + //Constructor RobotSim(void):tcp(0){} @@ -79,7 +82,7 @@ class RobotSim : public ComposedEntity //Forward and inverse kinematics Relative. The inverse kinematics must be defined for each new class of robot virtual bool forwardKinematics(const vector &_q, Transformation3D& t); //Inverse kinematics. abstract. - virtual bool inverseKinematics(Transformation3D t, vector &_q, unsigned char conf=0x00)=0; + virtual bool inverseKinematics(Transformation3D t06, vector &_q, unsigned char conf=NULL)=0; //Forward and inverse kinematics Absolute: generic methods. virtual bool forwardKinematicsAbs(vector _q, Transformation3D& t); @@ -93,7 +96,7 @@ class RobotSim : public ComposedEntity //Movements methods: generic but not safe bool moveTo(double *_q); //Simulation of time - virtual void simulate(double delta_t);//time interval in seconds + //virtual void simulate(double delta_t);//time interval in seconds //data interface int getNumJoints(){return (int)joints.size();} bool getJointLimits(int i, double &max, double &min){ @@ -111,6 +114,44 @@ class RobotSim : public ComposedEntity //Specific Collision checking bool checkRobotColision(); +//cinematic simulation methods + bool checkActuatorsIsMoving(); + void checkActivationTrajectory(); + virtual void goTo(vector _q); + virtual void simulate(double delta_t);//time interval in seconds + virtual void goToAbs(Transformation3D t); + //Load T3D relative + virtual void goTo(Transformation3D t); + +//Cubic Polinomial Trajectory (CPT) (ponit to point) + void calculateTargetCPT(double _time); + void calculateTargetTimeCPT(); + void activateCubicPolynomialTrajectory(){activateTVP=false;activateCPT=true; + for(int i=0;i<(int)actuators.size();i++) + actuators[i]->activateCubicPolynomialTrajectory();} + +//Trapezoidal Velocity Profile tarjectory (TVP) (point to point) + void calculateTargetTVP(double _time); + void calculateTargetTimeTVP(); + void adjustMotionJointsTVP (vector _path,double _targetTime,double _ta,int index); + void activateTrapezoidalVelocityTrajectory(){activateTVP=true;activateCPT=false; + for(int i=0;i<(int)actuators.size();i++) + actuators[i]->activateTrapezoidalVelocityTrajectory();} + +// Spline trajetory (interpolation points) + + //Thomas Algorithm for Tridiagonal Matrix + /* + b1 c1 0 + a2 b2 c2 + M = + cn-1 + 0 an bn + */ + vector TDMA (vector a, vector b, vector c,vector d, int nIterations); + void calculateTargetTimeSPLINE(); + void calculateTargetSPLINE(double _time); + protected: @@ -119,6 +160,22 @@ class RobotSim : public ComposedEntity vector joints; Tcp *tcp; + +//cinematic simulation atributes + vector actuators; + Actuator* actuator; + double time,targetTime; + bool activateTVP,activateCPT; + + unsigned char conf; + vector q_init; + vector q_target; + +//atributtes specific TVP movement + bool posInit; + double ta; + vector qInit; + }; };//end namespace mr diff --git a/src/world/simplejoint.cpp b/src/world/simplejoint.cpp index de80943..2af5777 100644 --- a/src/world/simplejoint.cpp +++ b/src/world/simplejoint.cpp @@ -42,7 +42,7 @@ void SimpleJoint::writeToStream(Stream& stream) Joint::writeToStream(stream); stream<<((char)axis); stream<>aux; axis=(Axis)aux; stream>>factor>>offset>>prismatic>>value>>vmax>>vmin; - stream>>speed>>maxSpeed; +// stream>>speed>>maxSpeed; +} + +void SimpleJoint::writeToXML(XMLElement* parent) +{ + //XMLAux aux; + //XMLElement* sJoint=new XMLElement(parent,"simplejoint"); + XMLVariable* ax= new XMLVariable ("axis",(const char*)axis); + XMLVariable* fac= new XMLVariable ("factor",XMLAux::string_Convert(factor).c_str()); + XMLVariable* offs= new XMLVariable ("offset",XMLAux::string_Convert(offset).c_str()); + XMLVariable* prismat= new XMLVariable ("prisamtic",XMLAux::string_ConvertBool(prismatic).c_str()); + XMLVariable* val= new XMLVariable ("value",XMLAux::string_Convert(value).c_str()); + XMLVariable* _vmax= new XMLVariable ("vmax",XMLAux::string_Convert(vmax).c_str()); + XMLVariable* _vmin= new XMLVariable ("vmin",XMLAux::string_Convert(vmin).c_str()); +// XMLVariable* veloc= new XMLVariable ("speed",XMLAux::string_Convert(speed).c_str()); +// XMLVariable* maxveloc= new XMLVariable ("maxSpeed",XMLAux::string_Convert(maxSpeed).c_str()); + + parent->AddVariable(ax); + parent->AddVariable(fac); + parent->AddVariable(offs); + parent->AddVariable(prismat); + parent->AddVariable(val); + parent->AddVariable(_vmax); + parent->AddVariable(_vmin); +// parent->AddVariable(veloc); +// parent->AddVariable(maxveloc); + //parent->AddElement(sJoint); + Joint::writeToXML(parent); +} + +void SimpleJoint::readFromXML(XMLElement* parent) +{ + + if (parent->FindVariableZ("axis")) + { + char cad; + parent->FindVariableZ("axis")->GetValue(&cad); + axis=(Axis)cad; + } + if (parent->FindVariableZ("factor")) + { + factor=XMLAux::GetValueDouble(parent->FindVariableZ("factor")); + } + if (parent->FindVariableZ("offset")) + { + offset=XMLAux::GetValueDouble(parent->FindVariableZ("offset")); + } + + if (parent->FindVariableZ("prismatic")) + { + string cad; + cad=XMLAux::GetValueCadena(parent->FindVariableZ("prismatic")); + + if (cad=="true" || cad!="0") + prismatic=true; + else + prismatic=false; + } + + if (parent->FindVariableZ("value")) + { + value=XMLAux::GetValueDouble(parent->FindVariableZ("value")); + } + + if (parent->FindVariableZ("vmax")) + { + vmax=XMLAux::GetValueDouble(parent->FindVariableZ("vmax")); + } + + if (parent->FindVariableZ("vmin")) + { + vmin=XMLAux::GetValueDouble(parent->FindVariableZ("vmin")); + } + + //if (parent->FindVariableZ("speed")) + //{ + // speed=XMLAux::GetValueDouble(parent->FindVariableZ("speed")); + //} + + //if (parent->FindVariableZ("maxSpeed")) + //{ + // maxSpeed=XMLAux::GetValueDouble(parent->FindVariableZ("maxSpeed")); + //} + + Joint::readFromXML(parent); + } ostream& operator<<(ostream& os, const SimpleJoint& p) @@ -86,29 +171,12 @@ void SimpleJoint::setProperties(double _max, double _min, bool CW, double _offse else factor=-1.0F; axis=ax; prismatic=_prismatic; - maxSpeed=1; // m/s rad/s - speed=1; //m/s rad/sec - target=value; - targetActive=false; } SimpleJoint::~SimpleJoint(void) { -} -void SimpleJoint::simulate(double delta_t) -{ -if(targetActive==false)return; -double d=target-value; -double inc=delta_t*speed; -if(d<0)inc=((-incd?d:inc); -if(fabs(inc)maxSpeed?maxSpeed:speed; - } - double setSpeed(double sp){if(sp<0)return speed; speed=sp>maxSpeed?maxSpeed:sp;return speed;} - double getSpeed(){return speed;} - double getMaxSpeed(){return maxSpeed;} - bool setTarget(double val){if((valvmax))return false;target=val;targetActive=true; - return true; - } - double getTarget(){return target;} - bool isMoving(){return targetActive;} - virtual void simulate(double delta_t); }; }; diff --git a/src/world/solidentity.cpp b/src/world/solidentity.cpp index 56aabb8..c74a473 100644 --- a/src/world/solidentity.cpp +++ b/src/world/solidentity.cpp @@ -59,6 +59,51 @@ SolidEntity::~SolidEntity(void) } +//serialization +void SolidEntity::writeToXML(XMLElement* parent) +{ + + if (!intersectable) //default value is true + { + XMLVariable* inter; + inter= new XMLVariable("intersectable", XMLAux::string_ConvertBool(intersectable).c_str()); + parent->AddVariable(inter); + } + + PositionableEntity::writeToXML(parent); +// box.writeToXML(parent); + material.writeToXML(parent); + + +} + +void SolidEntity::readFromXML(XMLElement* parent) +{ + //XMLElement* solid; + //if(parent->FindElementZ("solidentity")) + // solid=parent->FindElementZ("solidentity"); + //else + // solid=parent; + + if(parent->FindVariableZ("intersectable")) + { + string cad; + cad=XMLAux::GetValueCadena(parent->FindVariableZ("intersectable")); + + if (cad=="true" || cad!="0") + setIntersectable(true); + else + setIntersectable(false); + } + + PositionableEntity::readFromXML(parent); + box.readFromXML(parent); + material.readFromXML(parent); + setBoxNeedToBeUpdated(); + +} + + void SolidEntity::locationUpdated() { PositionableEntity::locationUpdated(); diff --git a/src/world/solidentity.h b/src/world/solidentity.h index 404b1b4..172924b 100644 --- a/src/world/solidentity.h +++ b/src/world/solidentity.h @@ -96,6 +96,9 @@ class SolidEntity: public PositionableEntity stream>>intersectable; setBoxNeedToBeUpdated(); } + void writeToXML(XMLElement* parent); + void readFromXML(XMLElement* parent); + //operator = const SolidEntity &operator=(const SolidEntity &s){ *((PositionableEntity *)this)=s; diff --git a/src/world/spherepart.cpp b/src/world/spherepart.cpp index a912e7e..7359f17 100644 --- a/src/world/spherepart.cpp +++ b/src/world/spherepart.cpp @@ -61,6 +61,60 @@ void SpherePart::readFromStream(Stream& stream) } +void SpherePart::writeToXML(XMLElement* parent) +{//radius=1.0, int _meridian=12, int _parallel=9 + //XMLAux aux; + + if (radius!=1.0) + { + XMLVariable* radio= new XMLVariable("radius",XMLAux::string_Convert(radius).c_str()); + parent->AddVariable(radio); + } + if (meridian!=12) + { + XMLVariable* paralelo=new XMLVariable("parallel",XMLAux::string_Convert(meridian).c_str()); + parent->AddVariable(paralelo); + } + + if (parallel!=9) + { + XMLVariable* meridiano=new XMLVariable("meridian",XMLAux::string_Convert(parallel).c_str()); + parent->AddVariable(meridiano); + } + + PrimitiveSolidEntity::writeToXML(parent); + +} + +void SpherePart::readFromXML(XMLElement* parent) +{ + + double rad=radius; + int par=parallel,mer=meridian; + + //XMLElement* sphere; + //if (parent->FindElementZ("SpherePart")) + // sphere=parent->FindElementZ("SpherePart"); + //else + // sphere=parent; + + if(parent->FindVariableZ("radius")) + rad=XMLAux::GetValueDouble(parent->FindVariableZ("radius")); + + if(parent->FindVariableZ("parallel")) + par=parent->FindVariableZ("parallel")->GetValueInt(); + + if(parent->FindVariableZ("meridian")) + mer=parent->FindVariableZ("meridian")->GetValueInt(); + + PrimitiveSolidEntity::readFromXML(parent); + + setNumVertex(mer,par); + setRadius(rad); + +} + + ostream& operator<<(ostream& os, const SpherePart& s) { //os<