diff --git a/raisim/m1/include/raisim/RaiSimTinyXmlWriter.hpp b/raisim/m1/include/raisim/RaiSimTinyXmlWriter.hpp index 413a6fe2..763fdcfd 100644 --- a/raisim/m1/include/raisim/RaiSimTinyXmlWriter.hpp +++ b/raisim/m1/include/raisim/RaiSimTinyXmlWriter.hpp @@ -75,6 +75,17 @@ class RaiSimTinyXmlWriterElement { return *this; } + template + RaiSimTinyXmlWriterElement& setVectorAttribute(const std::string &name, T attribute, size_t size) { + std::string list; + for(int i=0; iSetAttribute(name, list); + return *this; + } + void link() { for(auto& element : elements_) { ptr_->LinkEndChild(element.ptr_); diff --git a/raisim/m1/include/raisim/RaisimServer.hpp b/raisim/m1/include/raisim/RaisimServer.hpp index 8a05131b..81bb6c44 100644 --- a/raisim/m1/include/raisim/RaisimServer.hpp +++ b/raisim/m1/include/raisim/RaisimServer.hpp @@ -62,30 +62,13 @@ class RaisimServer final { static constexpr int RECEIVE_BUFFER_SIZE = 33554432; enum ClientMessageType : int { - REQUEST_OBJECT_POSITION = 0, - REQUEST_INITIALIZATION, - REQUEST_RESOURCE, // request mesh, texture. etc files - REQUEST_CHANGE_REALTIME_FACTOR, - REQUEST_CONTACT_SOLVER_DETAILS, - REQUEST_PAUSE, - REQUEST_RESUME, - REQUEST_CONTACT_INFOS, - REQUEST_CONFIG_XML, - REQUEST_INITIALIZE_VISUALS, - REQUEST_VISUAL_POSITION, - REQUEST_SERVER_STATUS, + REQUEST_UPDATE = 0, REQUEST_SENSOR_UPDATE }; enum ServerMessageType : int { - INITIALIZATION = 0, - OBJECT_POSITION_UPDATE = 1, - STATUS = 2, - NO_MESSAGE = 3, - CONTACT_INFO_UPDATE = 4, - CONFIG_XML = 5, - VISUAL_INITILIZATION = 6, - VISUAL_POSITION_UPDATE = 7 + UPDATE_ALL = 0, + No_MESSAGE }; enum class ServerRequestType : int { @@ -104,6 +87,18 @@ class RaisimServer final { STATUS_TERMINATING = 2 }; + enum Masking : int { + AS_VISUAL_OBJ = 0, + AS_COL_OBJ, + SB_OBJ, + VIS_OBJ, + CONSTRAINTS, + CONTACT_FORCE, + CONTACT_POINT, + EXT_FORCE, + EXT_TORQUE + }; + /** * @param[in] world the world to visualize. * create a raisimSever for a world. */ @@ -121,20 +116,20 @@ class RaisimServer final { void setupSocket() { #if __linux__ || __APPLE__ int opt = 1; - int addrlen = sizeof(address); + addrlen = sizeof(address); // Creating socket file descriptor - RSFATAL_IF((server_fd_ = socket(AF_INET, SOCK_STREAM, 0)) == 0, "socket error: " << strerror(errno)) + RSFATAL_IF((server_fd_ = socket(AF_INET, SOCK_STREAM, 0)) < 0, "socket error, errno: " << errno) RSFATAL_IF(setsockopt(server_fd_, SOL_SOCKET, RAISIM_SERVER_SOCKET_OPTION, - (char *) &opt, sizeof(opt)), "setsockopt error: "<< strerror(errno)) + (char *) &opt, sizeof(opt)) == -1, "setsockopt error, errno: " << errno) address.sin_family = AF_INET; address.sin_addr.s_addr = INADDR_ANY; address.sin_port = htons(raisimPort_); // Forcefully attaching socket to the port 8080 - RSFATAL_IF(bind(server_fd_, (struct sockaddr *) &address, sizeof(address)) < 0, "bind error: " << strerror(errno)) - RSFATAL_IF(listen(server_fd_, 3) < 0, "listen error: " << strerror(errno)) + RSFATAL_IF(bind(server_fd_, (struct sockaddr *) &address, sizeof(address)) < 0, "bind error, errno: " << errno) + RSFATAL_IF(listen(server_fd_, 3) < 0, "listen error, errno: " << errno) #elif WIN32 WSADATA wsaData; @@ -176,11 +171,11 @@ class RaisimServer final { return; } - setsockopt(server_fd_, SOL_SOCKET, SO_SNDBUF, (char *)&opt, sizeof(opt)); - setsockopt(server_fd_, SOL_SOCKET, SO_RCVBUF, (char *)&opt, sizeof(opt)); + setsockopt(server_fd_, SOL_SOCKET, SO_SNDBUF, (char *) &opt, sizeof(opt)); + setsockopt(server_fd_, SOL_SOCKET, SO_RCVBUF, (char *) &opt, sizeof(opt)); // Setup the TCP listening socket - iResult = bind(server_fd_, result->ai_addr, (int)result->ai_addrlen); + iResult = bind(server_fd_, result->ai_addr, (int) result->ai_addrlen); if (iResult == SOCKET_ERROR) { printf("bind failed with error: %d\n", WSAGetLastError()); closesocket(server_fd_); @@ -205,13 +200,14 @@ class RaisimServer final { void acceptConnection(int seconds) { if (waitForNewClients(seconds)) { #if __linux__ || __APPLE__ - RSFATAL_IF((client_ = accept(server_fd_, (struct sockaddr *) &address, - (socklen_t *) &addrlen)) < 0, "accept failed") + client_ = accept(server_fd_, (struct sockaddr *) &address, (socklen_t *) &addrlen); connected_ = client_ > -1; #elif WIN32 - client_ = int(accept(server_fd_, NULL, NULL)); + client_ = int(accept(server_fd_, nullptr, nullptr)); connected_ = client_ != INVALID_SOCKET; #endif + RSWARN_IF(client_ < 0, "Accept failed, errno: " << errno) + clearScene(); } } @@ -229,11 +225,23 @@ class RaisimServer final { private: + inline void clearScene() { + lockVisualizationServerMutex(); + for (auto ob : world_->getObjList()) ob->visualTag = 0u; + for (auto& ob: world_->getWires()) ob->visualTag = 0u; + for (auto& ob: visuals_) ob.second->visualTag = 0u; + for (auto& ob: visualAs_) ob.second->obj.visualTag = 0u; + for (auto& ob: instancedvisuals_) ob.second->visualTag = 0u; + for (auto& ob: polyLines_) ob.second->visualTag = 0u; + for (auto& ob: charts_) ob.second->visualTag = 0u; + unlockVisualizationServerMutex(); + } + inline void loop() { setupSocket(); while (!terminateRequested_) { - acceptConnection(2.0); + acceptConnection(2); while (connected_) { connected_ = processRequests() && !terminateRequested_; @@ -251,6 +259,16 @@ class RaisimServer final { } public: + /** + * @param[in] map name of the map to be loaded. This only works with RaisimUnreal. Currently, the following maps are available + * "": Empty map. Supports weather and time. + * "simple": Empty map. Simple sky for faster rendering. + * "wheat": a flat wheat field + * "dune": flat dune + */ + inline void setMap(const std::string &map) { + mapName_ = map; + } /** * @param[in] port port number to stream @@ -323,6 +341,7 @@ class RaisimServer final { if (visualAs_.find(name) != visualAs_.end()) RSFATAL("Duplicated visual object name: " + name) visualAs_[name] = new ArticulatedSystemVisual(urdfFile); visualAs_[name]->color = raisim::Vec<4>{colorR, colorG, colorB, colorA}; + visualAs_[name]->name = name; updateVisualConfig(); return visualAs_[name]; } @@ -330,12 +349,12 @@ class RaisimServer final { /** * @param[in] as ArticulatedSystemVisual to be removed * remove a visualized articulated system */ - inline void removeVisualArticulatedSystem(ArticulatedSystemVisual* as) { + inline void removeVisualArticulatedSystem(ArticulatedSystemVisual *as) { auto it = visualAs_.begin(); // Search for an element with value 2 - while(it != visualAs_.end()) { - if(it->second == as) + while (it != visualAs_.end()) { + if (it->second == as) break; it++; } @@ -348,10 +367,10 @@ class RaisimServer final { } inline InstancedVisuals *addInstancedVisuals(const std::string &name, - InstancedVisuals::VisualType type, - const Vec<3>& size, - const Vec<4>& color1, - const Vec<4>& color2) { + Shape::Type type, + const Vec<3> &size, + const Vec<4> &color1, + const Vec<4> &color2) { RSFATAL_IF(instancedvisuals_.find(name) != instancedvisuals_.end(), "Duplicated visual object name: " + name) updateVisualConfig(); instancedvisuals_[name] = new InstancedVisuals(type, name, size, color1, color2); @@ -378,7 +397,7 @@ class RaisimServer final { if (visuals_.find(name) != visuals_.end()) RSFATAL("Duplicated visual object name: " + name) updateVisualConfig(); visuals_[name] = new Visuals(); - visuals_[name]->type = Visuals::VisualType::VisualSphere; + visuals_[name]->type = Shape::Sphere; visuals_[name]->name = name; visuals_[name]->size[0] = radius; visuals_[name]->color = {colorR, colorG, colorB, colorA}; @@ -410,7 +429,7 @@ class RaisimServer final { if (visuals_.find(name) != visuals_.end()) RSFATAL("Duplicated visual object name: " + name) updateVisualConfig(); visuals_[name] = new Visuals(); - visuals_[name]->type = Visuals::VisualType::VisualBox; + visuals_[name]->type = Shape::Box; visuals_[name]->name = name; visuals_[name]->size[0] = xLength; visuals_[name]->size[1] = yLength; @@ -443,7 +462,7 @@ class RaisimServer final { if (visuals_.find(name) != visuals_.end()) RSFATAL("Duplicated visual object name: " + name) updateVisualConfig(); visuals_[name] = new Visuals(); - visuals_[name]->type = Visuals::VisualType::VisualCylinder; + visuals_[name]->type = Shape::Cylinder; visuals_[name]->name = name; visuals_[name]->size[0] = radius; visuals_[name]->size[1] = length; @@ -475,7 +494,7 @@ class RaisimServer final { if (visuals_.find(name) != visuals_.end()) RSFATAL("Duplicated visual object name: " + name) updateVisualConfig(); visuals_[name] = new Visuals(); - visuals_[name]->type = Visuals::VisualType::VisualCapsule; + visuals_[name]->type = Shape::Capsule; visuals_[name]->name = name; visuals_[name]->size[0] = radius; visuals_[name]->size[1] = length; @@ -506,7 +525,7 @@ class RaisimServer final { if (visuals_.find(name) != visuals_.end()) RSFATAL("Duplicated visual object name: " + name) updateVisualConfig(); visuals_[name] = new Visuals(); - visuals_[name]->type = Visuals::VisualType::VisualMesh; + visuals_[name]->type = Shape::Mesh; visuals_[name]->name = name; visuals_[name]->meshFileName = file; visuals_[name]->size = scale; @@ -516,7 +535,6 @@ class RaisimServer final { return visuals_[name]; } - /** * @param[in] name the name of the visual mesh object * @param[in] radius radius of the arrow @@ -537,7 +555,7 @@ class RaisimServer final { if (visuals_.find(name) != visuals_.end()) RSFATAL("Duplicated visual object name: " + name) updateVisualConfig(); visuals_[name] = new Visuals(); - visuals_[name]->type = Visuals::VisualType::VisualArrow; + visuals_[name]->type = Shape::Arrow; visuals_[name]->name = name; visuals_[name]->size[0] = radius; visuals_[name]->size[1] = height; @@ -638,7 +656,7 @@ class RaisimServer final { void focusOn(raisim::Object *obj) { RSFATAL_IF(obj == nullptr, "object does not exist.") serverRequest_.push_back(ServerRequestType::FOCUS_ON_SPECIFIC_OBJECT); - focusedObjectName_ = obj->getName(); + focusedObjectVisTag_ = obj->visualTag; } /** @@ -667,18 +685,18 @@ class RaisimServer final { } return true; #elif WIN32 - fd_set fds ; - int n ; - struct timeval tv ; - FD_ZERO(&fds) ; - FD_SET(client_, &fds) ; - tv.tv_sec = 20 ; - tv.tv_usec = 100000 ; - n = select ( server_fd_+1, &fds, NULL, NULL, &tv ) ; - if ( n == 0) { - RSWARN("The client failed to respond in "<< seconds <<" seconds. Looking for a new client"); + fd_set fds; + int n; + struct timeval tv; + FD_ZERO(&fds); + FD_SET(client_, &fds); + tv.tv_sec = 20; + tv.tv_usec = 100000; + n = select(server_fd_ + 1, &fds, NULL, NULL, &tv); + if (n == 0) { + RSWARN("The client failed to respond in " << seconds << " seconds. Looking for a new client"); return false; - } else if( n == -1 ) { + } else if (n == -1) { RSWARN("The client error. Failed to communicate."); return false; } @@ -702,18 +720,18 @@ class RaisimServer final { } return true; #elif WIN32 - fd_set fds ; - int n ; - struct timeval tv ; - FD_ZERO(&fds) ; - FD_SET(client_, &fds) ; - tv.tv_sec = 20 ; - tv.tv_usec = 100000 ; - n = select ( server_fd_+1, NULL, &fds, NULL, &tv ) ; - if ( n == 0) { - RSWARN("The client failed to respond in " << seconds <<" seconds. Looking for a new client"); + fd_set fds; + int n; + struct timeval tv; + FD_ZERO(&fds); + FD_SET(client_, &fds); + tv.tv_sec = 20; + tv.tv_usec = 100000; + n = select(server_fd_ + 1, NULL, &fds, NULL, &tv); + if (n == 0) { + RSWARN("The client failed to respond in " << seconds << " seconds. Looking for a new client"); return false; - } else if( n == -1 ) { + } else if (n == -1) { RSWARN("The client error. Failed to communicate."); return false; } @@ -721,6 +739,12 @@ class RaisimServer final { #endif } + /** + * Synchronous update method. + * Receive a request from the client, process it and return the requested data to the client. + * The method return false if 1) the client failed to respond 2) the client protocol version is different 3) the client refused to receive the data 4) the client did not send the sensor data in time + * @return if succeeded or not. + */ inline bool processRequests() { using namespace server; ClientMessageType type; @@ -728,7 +752,7 @@ class RaisimServer final { int clientVersion; rData_ = get(rData_, &clientVersion); - data_ = set(&send_buffer[0]+sizeof(int), version_); + data_ = set(&send_buffer[0] + sizeof(int), version_); if (clientVersion == version_) { rData_ = get(rData_, &type, &objectId_); @@ -736,7 +760,7 @@ class RaisimServer final { // set server request data_ = set(data_, (uint64_t) serverRequest_.size()); - for (const auto &sr : serverRequest_) { + for (const auto &sr: serverRequest_) { data_ = set(data_, (int) sr); switch (sr) { @@ -754,7 +778,7 @@ class RaisimServer final { break; case ServerRequestType::FOCUS_ON_SPECIFIC_OBJECT: - data_ = set(data_, focusedObjectName_); + data_ = set(data_, focusedObjectVisTag_); break; case ServerRequestType::SET_SCREEN_SIZE: @@ -766,35 +790,8 @@ class RaisimServer final { serverRequest_.clear(); lockVisualizationServerMutex(); - if (state_ != Status::STATUS_HIBERNATING) { - switch (type) { - case REQUEST_OBJECT_POSITION: - serializeWorld(); - break; - - case REQUEST_INITIALIZATION: - serializeObjects(); - serializeVisuals(); - serializeWorld(); - break; - - case REQUEST_CHANGE_REALTIME_FACTOR: - break; - - case REQUEST_CONTACT_INFOS: - serializeContacts(); - break; + if (state_ != Status::STATUS_HIBERNATING) update(); - case REQUEST_CONFIG_XML: - case REQUEST_INITIALIZE_VISUALS: - case REQUEST_VISUAL_POSITION: - case REQUEST_SERVER_STATUS: - return false; - - default: - break; - } - } unlockVisualizationServerMutex(); } else { RSWARN("Version mismatch. Make sure you have the correct visualizer version") @@ -815,7 +812,12 @@ class RaisimServer final { return state_ == STATUS_RENDERING || state_ == STATUS_HIBERNATING; } - inline bool waitForNewClients(int seconds) { + /** + * wait for a new client + * @param seconds how long to wait for a new client + * @return if a new client was found or not + */ + inline bool waitForNewClients(int seconds) { fd_set sdset; struct timeval tv; tv.tv_sec = seconds; @@ -827,234 +829,420 @@ class RaisimServer final { private: - inline void serializeWorld() { - using namespace server; - auto &objList = world_->getObjList(); - data_ = set(data_, ServerMessageType::OBJECT_POSITION_UPDATE); - data_ = set(data_, (uint64_t) world_->getConfigurationNumber() + visualConfiguration_); - data_ = set(data_, (uint64_t) (objList.size())); + static inline std::string colorToString(const raisim::Vec<4> &vec) { + std::string str; + for (int i = 0; i < vec.size() - 1; i++) { + str += std::to_string(vec[i]) + ", "; + } + str += std::to_string(vec[vec.size() - 1]); + return str; + } - for (auto *ob : objList) { - // set gc - if (ob->getObjectType() == ObjectType::ARTICULATED_SYSTEM) { - auto as = dynamic_cast(ob); - data_ = set(data_, (uint64_t) (as->getVisOb().size() + - as->getVisColOb().size())); - data_ = set(data_, (uint64_t) as->getSensors().size()); - - auto& visOb = as->getVisOb(); - for (uint64_t k = 0; k < visOb.size(); k++) { - auto &vob = visOb[k]; - std::string name = std::to_string(ob->getIndexInWorld()) + - "/" + std::to_string(0) + "/" + - std::to_string(k); - data_ = set(data_, name); - - Vec<3> pos, offsetInWorld; - Vec<4> quat; - Mat<3, 3> bodyRotation, rot; - ob->getPosition(vob.localIdx, pos); - ob->getOrientation(vob.localIdx, bodyRotation); - matvecmul(bodyRotation, vob.offset, offsetInWorld); - matmul(bodyRotation, vob.rot, rot); - raisim::rotMatToQuat(rot, quat); - pos = pos + offsetInWorld; - data_ = set(data_, pos, quat); + inline void serializeAS(ArticulatedSystem* as, bool initialized, const raisim::Vec<4>& colorOverride) { + using namespace server; + data_ = set(data_, (int32_t) (as->getVisOb().size() + as->getVisColOb().size())); + if (!initialized) data_ = set(data_, as->name_); + + for (int i = 0; i < 2; i++) { + std::vector *visVec; + if (i == 0) visVec = &as->getVisOb(); + else visVec = &as->getVisColOb(); + + for (int j = 0; j < visVec->size(); j++) { + if (!initialized) { + data_ = set(data_, visVec->at(j).shape); + if (visVec->at(j).shape == Shape::Mesh) { + data_ = set(data_, visVec->at(j).fileName); + data_ = set(data_, as->getResourceDir()); + } + data_ = set(data_, i); } - auto& colOb = as->getCollisionBodies(); - for (uint64_t k = 0; k < colOb.size(); k++) { - auto &vob = colOb[k]; - std::string name = std::to_string(ob->getIndexInWorld()) + - "/" + std::to_string(1) + "/" + - std::to_string(k); - data_ = set(data_, name); + if (colorOverride[3] < 0.001) + data_ = set(data_, colorToString(visVec->at(j).color)); + else + data_ = set(data_, colorToString(colorOverride)); - Vec<3> pos, offsetInWorld; - Vec<4> quat; - Mat<3, 3> bodyRotation, rot; - ob->getPosition(vob.localIdx, pos); - ob->getOrientation(vob.localIdx, bodyRotation); - matvecmul(bodyRotation, vob.posOffset, offsetInWorld); - matmul(bodyRotation, vob.rotOffset, rot); - raisim::rotMatToQuat(rot, quat); - pos = pos + offsetInWorld; - data_ = set(data_, pos, quat); - } + if (visVec->at(j).shape == Shape::Mesh) + data_ = setInFloat(data_, visVec->at(j).scale, 0.); + else + data_ = setInFloat(data_, visVec->at(j).visShapeParam); - // add sensors to be updated - for (auto& sensor: as->getSensors()) { - if (sensor.second->getMeasurementSource() == Sensor::MeasurementSource::VISUALIZER && - sensor.second->getUpdateTimeStamp() + 1. / sensor.second->getUpdateRate() < world_->getWorldTime()) { - sensor.second->setUpdateTimeStamp(world_->getWorldTime()); - sensor.second->updatePose(); - Vec<4> quat; - auto& pos = sensor.second->getPosition(); - auto& rot = sensor.second->getOrientation(); - rotMatToQuat(rot, quat); - data_ = set(data_, int(1)); - data_ = setInFloat(data_, pos, quat); - needsSensorUpdate_ = true; - } else { - data_ = set(data_, int(0)); - } + Vec<3> pos, offsetInWorld, posOffset; + Vec<4> quat; + Mat<3, 3> oriOffset, bodyRotation, rot; + as->getPosition(visVec->at(j).localIdx, pos); + as->getOrientation(visVec->at(j).localIdx, bodyRotation); + + if (i == 0) { + posOffset = visVec->at(j).offset; + oriOffset = visVec->at(j).rot; + } else { + posOffset = as->getCollisionBodies()[j].posOffset; + oriOffset = as->getCollisionBodies()[j].rotOffset; } + + matvecmul(bodyRotation, posOffset, offsetInWorld); + matmul(bodyRotation, oriOffset, rot); + raisim::rotMatToQuat(rot, quat); + pos = pos + offsetInWorld; + data_ = setInFloat(data_, pos, quat); + } + } + + data_ = set(data_, (int32_t) as->getSensors().size()); + // add sensors to be updated + for (auto &sensor: as->getSensors()) { + if (!initialized) data_ = sensor.second->serializeProp(data_); + + if (sensor.second->getMeasurementSource() == Sensor::MeasurementSource::VISUALIZER && + sensor.second->getUpdateTimeStamp() + 1. / sensor.second->getUpdateRate() + < world_->getWorldTime() + 1e-10) { + sensor.second->setUpdateTimeStamp(world_->getWorldTime()); + sensor.second->updatePose(); + Vec<4> quat; + auto &pos = sensor.second->getPosition(); + auto &rot = sensor.second->getOrientation(); + rotMatToQuat(rot, quat); + data_ = set(data_, true); + data_ = setInFloat(data_, pos, quat); + needsSensorUpdate_ = true; + } else { + data_ = set(data_, false); + } + } + } + + inline void update() { + using namespace server; + auto &objList = world_->getObjList(); + data_ = set(data_, ServerMessageType::UPDATE_ALL); + data_ = set(data_, mapName_); + data_ = set(data_, (uint32_t) (world_->getConfigurationNumber() + visualConfiguration_)); + data_ = set(data_, (uint32_t) (objList.size() + + visuals_.size() + + instancedvisuals_.size() + + visualAs_.size() + + polyLines_.size() + + world_->getWires().size())); + + /// UniqueVisualTag/IsInitialized/Name/IsVisual/ObjectType/Instanced/ + /// VisualSize/{IfNotInitialized:[VisualType/VisualDescription/Masking]/ + /// Appearance/Size/Pose}/ + /// SensorSize/{SensorDescription} + for (auto *ob: objList) { + // set gc + bool initialized = ob->visualTag != 0; + if (!initialized) ob->visualTag = visTagCounter++; + data_ = set(data_, ob->visualTag, initialized, ob->getObjectType(), false); + if (ob->getObjectType() == ObjectType::ARTICULATED_SYSTEM) { + auto as = dynamic_cast(ob); + Vec<4> colorOverride; + colorOverride.setZero(); + serializeAS(as, initialized, colorOverride); } else if (ob->getObjectType() == ObjectType::COMPOUND) { auto *com = dynamic_cast(ob); - data_ = set(data_, (uint64_t) (com->getObjList().size())); - data_ = set(data_, (uint64_t) 0); // sensor size + data_ = set(data_, (int32_t) (com->getObjList().size())); + if (!initialized ) data_ = set(data_, ob->name_); + + for (auto &vob: dynamic_cast(ob)->getObjList()) { + if (!initialized) { + switch (vob.objectType) { + case BOX: + data_ = set(data_, Shape::Box); + break; + case CAPSULE: + data_ = set(data_, Shape::Capsule); + break; + case CYLINDER: + data_ = set(data_, Shape::Cylinder); + break; + case SPHERE: + data_ = set(data_, Shape::Sphere); + break; + default: + break; + } + data_ = set(data_, Masking::SB_OBJ); + } + data_ = set(data_, vob.appearance); + data_ = setInFloat(data_, vob.objectParam); - for (uint64_t k = 0; k < com->getObjList().size(); k++) { - std::string name = std::to_string(ob->getIndexInWorld()) + "/" + std::to_string(k); - data_ = set(data_, name); Vec<3> pos, center; com->getPosition(center); - matvecmul(com->getRotationMatrix(), com->getObjList()[k].trans.pos, pos); + matvecmul(com->getRotationMatrix(), vob.trans.pos, pos); Mat<3, 3> rot; - matmul(com->getRotationMatrix(), com->getObjList()[k].trans.rot, rot); + matmul(com->getRotationMatrix(), vob.trans.rot, rot); Vec<4> quat; raisim::rotMatToQuat(rot, quat); pos = pos + center; - data_ = set(data_, pos, quat); + data_ = setInFloat(data_, pos, quat); } + data_ = set(data_, (int32_t) 0); } else { - data_ = set(data_, (uint64_t) (1)); - data_ = set(data_, (uint64_t) 0); // sensor size + data_ = set(data_, (int32_t) 1); + + if (!initialized) { + data_ = set(data_, ob->name_); + switch (ob->getObjectType()) { + case SPHERE: + data_ = set(data_, Shape::Sphere); + break; + case BOX: + data_ = set(data_, Shape::Box); + break; + case CYLINDER: + data_ = set(data_, Shape::Cylinder); + break; + case CONE: + break; + case CAPSULE: + data_ = set(data_, Shape::Capsule); + break; + case HALFSPACE: + data_ = set(data_, Shape::Ground); + break; + case MESH: + data_ = set(data_, Shape::Mesh); + data_ = set(data_, dynamic_cast(ob)->getMeshFileName()); + data_ = set(data_, std::string()); + break; + case HEIGHTMAP: { + auto hm = dynamic_cast(ob); + data_ = set(data_, Shape::HeightMap); + data_ = setInFloat(data_, hm->getCenterX(), hm->getCenterY(), hm->getXSize(), hm->getYSize()); + data_ = set(data_, (int32_t) hm->getXSamples(), (int32_t) hm->getYSamples()); + data_ = setInFloat(data_, hm->getHeightVector()); + } + case COMPOUND: + case ARTICULATED_SYSTEM: + case UNRECOGNIZED: + break; + } + data_ = set(data_, Masking::SB_OBJ); + } + auto *sob = dynamic_cast(ob); + + auto tempAdd = data_; + data_ = set(data_, sob->getAppearance()); + + switch (ob->getObjectType()) { + case SPHERE: + data_ = setInFloat(data_, dynamic_cast(ob)->getRadius(), 0., 0., 0.); + break; + case BOX: + data_ = setInFloat(data_, dynamic_cast(ob)->getDim(), 0.); + break; + case CYLINDER: + data_ = setInFloat(data_, + dynamic_cast(ob)->getRadius(), + dynamic_cast(ob)->getHeight(), + 0., + 0.); + break; + case CONE: + break; + case CAPSULE: + data_ = setInFloat(data_, + dynamic_cast(ob)->getRadius(), + dynamic_cast(ob)->getHeight(), + 0., + 0.); + break; + case HALFSPACE: + data_ = setInFloat(data_, dynamic_cast(ob)->getHeight(), 0., 0., 0.); + break; + case HEIGHTMAP: + data_ = set(data_, 0.f, 0.f, 0.f, 0.f); + break; + case MESH: { + auto scale = float(dynamic_cast(ob)->getScale()); + data_ = set(data_, scale, scale, scale, 0.f); + } + break; + case COMPOUND: + case ARTICULATED_SYSTEM: + case UNRECOGNIZED: + break; + } + Vec<3> pos; Vec<4> quat; - std::string name = std::to_string(ob->getIndexInWorld()); - data_ = set(data_, name); - dynamic_cast(ob)->getPosition(pos); - dynamic_cast(ob)->getQuaternion(quat); - data_ = set(data_, pos, quat); + sob->getPosition(pos); + sob->getQuaternion(quat); + data_ = setInFloat(data_, pos, quat); + data_ = set(data_, (int32_t) 0); } } - // visuals - data_ = set(data_, (uint64_t) (visuals_.size())); - data_ = set(data_, (uint64_t) (instancedvisuals_.size())); - data_ = set(data_, (uint64_t) (visualAs_.size())); + for (auto &sw: world_->getWires()) { + bool initialized = sw->visualTag != 0; + if (!initialized) sw->visualTag = visTagCounter++; + data_ = set(data_, sw->visualTag, initialized, int32_t(-1), false, (int32_t)1); + if (!initialized) data_ = set(data_, sw->name_, Shape::SingleLine, Masking::CONSTRAINTS); + data_ = set(data_, colorToString(sw->getColor())); + Vec<3> pos, diff, diff_norm; + Vec<4> quat; + Mat<3,3> rot; + diff = sw->getP2() - sw->getP1(); + diff_norm = diff * (1./diff.norm()); + pos = (sw->getP1() + sw->getP2()) / 2.0; + raisim::zaxisToRotMat(diff_norm, rot); + raisim::rotMatToQuat(rot, quat); + data_ = set(data_, 0.02f, 0.02f, (float)diff.norm(), 0.f); + data_ = setInFloat(data_, pos, quat); + data_ = set(data_, (int32_t) 0); + } // single visuals - for (auto &kAndVo : visuals_) { - auto *vo = kAndVo.second; + for (auto &ob: visuals_) { + auto *vo = ob.second; + bool initialized = vo->visualTag != 0; + if (!initialized) vo->visualTag = visTagCounter++; Vec<3> pos = vo->getPosition(); Vec<4> quat = vo->getOrientation(); - data_ = set(data_, vo->name, pos, quat, vo->type, vo->color, vo->size); - } - - // instanced visuals - for (auto &iv: instancedvisuals_) { - auto* v = iv.second; - data_ = set(data_, (uint64_t) v->count(), v->name); - for (size_t i=0; i < v->count(); i++) { - data_ = setInFloat(data_, v->data[i].pos, v->data[i].quat, v->data[i].scale); - data_ = set(data_, v->data[i].colorWeight); + data_ = set(data_, vo->visualTag, initialized, int32_t(-1), false, int32_t(1)); + if (!initialized) { + data_ = set(data_, vo->name, vo->type); + if (vo->type == Shape::Mesh) { + data_ = set(data_, vo->meshFileName); + data_ = set(data_, std::string()); + } + data_ = set(data_, Masking::VIS_OBJ); } + data_ = set(data_, colorToString(vo->color)); + data_ = setInFloat(data_, vo->size, pos, quat); + data_ = set(data_, (int32_t) 0); } // ArticulatedSystemVisuals - for (auto &vis : visualAs_) { + for (auto &vis: visualAs_) { auto *ob = &vis.second->obj; - data_ = set(data_, vis.second->color); - data_ = set(data_, (uint64_t) ob->getVisOb().size() + ob->getVisColOb().size()); - - for (uint64_t i = 0; i < 2; i++) { - std::vector *visOb; - if (i == 0) - visOb = &(ob->getVisOb()); - else - visOb = &(ob->getVisColOb()); - - for (uint64_t k = 0; k < (*visOb).size(); k++) { - auto &vob = (*visOb)[k]; - std::string name = vis.first + "/" + std::to_string(i) + "/" + std::to_string(k); - data_ = set(data_, name); - - Vec<3> pos, offsetInWorld; - Vec<4> quat; - Mat<3,3> bodyRotation, rot; + bool initialized = ob->visualTag != 0; + if (!initialized) ob->visualTag = visTagCounter++; + data_ = set(data_, ob->visualTag, initialized, ob->getObjectType(), false); + serializeAS(ob, initialized, vis.second->color); + } - ob->getPosition(vob.localIdx, pos); - ob->getOrientation(vob.localIdx, bodyRotation); - matvecmul(bodyRotation, vob.offset, offsetInWorld); - matmul(bodyRotation, vob.rot, rot); - pos = pos + offsetInWorld; - raisim::rotMatToQuat(rot, quat); - data_ = set(data_, pos); - data_ = set(data_, quat); - } + // instanced visuals + for (auto &iv: instancedvisuals_) { + auto *v = iv.second; + bool initialized = v->visualTag != 0; + if (!initialized) v->visualTag = visTagCounter++; + data_ = set(data_, v->visualTag, initialized, int32_t(-1), true, (int32_t) v->count()); + if (!initialized) data_ = set(data_, v->name, v->type, Masking::VIS_OBJ); + data_ = setInFloat(data_, v->color1, v->color2); + for (size_t i = 0; i < v->count(); i++) { + data_ = set(data_, v->data[i].colorWeight); + data_ = setInFloat(data_, v->data[i].scale, v->data[i].pos, v->data[i].quat); } } // polylines - data_ = set(data_, (uint64_t) (polyLines_.size())); - for (auto &pl : polyLines_) { + for (auto &pl: polyLines_) { auto *ptr = pl.second; - data_ = set(data_, ptr->name, ptr->color, ptr->width, (uint64_t) (ptr->points.size())); - for (auto& p : ptr->points) - data_ = set(data_, p); + bool initialized = ptr->visualTag != 0; + if (!initialized) ptr->visualTag = visTagCounter++; + data_ = set(data_, ptr->visualTag, initialized, int32_t(-1), true, (int32_t) (ptr->points.size()-1)); + if (!initialized) data_ = set(data_, ptr->name, Shape::PolyLine, Masking::VIS_OBJ); + data_ = setInFloat(data_, ptr->color, ptr->color); + RSFATAL_IF(ptr->points.size() < 2, "polyline point size should be greater than 1") + for (int i=0; ipoints.size() - 1; i++) { + Vec<3> pos, diff, norm_diff; + Vec<4> quat; + Mat<3,3> rot; + pos = (ptr->points[i] + ptr->points[i+1]) / 2.0; + diff = ptr->points[i+1] - ptr->points[i]; + norm_diff = diff / diff.norm(); + raisim::zaxisToRotMat(norm_diff, rot); + raisim::rotMatToQuat(rot, quat); + data_ = setInFloat(data_, 0., ptr->width, ptr->width, diff.norm()); + data_ = setInFloat(data_, pos, quat); + } } - // wires - data_ = set(data_, (uint64_t) (world_->getWires().size())); - for (auto &sw: world_->getWires()) - data_ = set(data_, sw->getP1(), sw->getP2()); - // External forces - size_t numExtForce = 0; - size_t numExtTorque = 0; + int32_t numExtForce = 0; + int32_t numExtTorque = 0; for (auto *ob: world_->getObjList()) { - numExtForce += ob->getExternalForce().size(); - numExtTorque += ob->getExternalTorque().size(); + numExtForce += int32_t(ob->getExternalForce().size()); + numExtTorque += int32_t(ob->getExternalTorque().size()); } - data_ = set(data_, (uint64_t) numExtForce); - numExtForce = 0; + data_ = set(data_, numExtForce); for (auto *ob: world_->getObjList()) { for (size_t extNum = 0; extNum < ob->getExternalForce().size(); extNum++) { - numExtForce++; - data_ = set(data_, ob->getExternalForcePosition()[extNum]); - data_ = set(data_, ob->getExternalForce()[extNum]); + data_ = setInFloat(data_, ob->getExternalForcePosition()[extNum]); + data_ = setInFloat(data_, ob->getExternalForce()[extNum]); } } - data_ = set(data_, (uint64_t) numExtTorque); + data_ = set(data_, numExtTorque); for (auto *ob: world_->getObjList()) { for (size_t extNum = 0; extNum < ob->getExternalTorque().size(); extNum++) { - data_ = set(data_, ob->getExternalTorquePosition()[extNum]); - data_ = set(data_, ob->getExternalTorque()[extNum]); + data_ = setInFloat(data_, ob->getExternalTorquePosition()[extNum]); + data_ = setInFloat(data_, ob->getExternalTorque()[extNum]); + } + } + + auto *contactList = world_->getContactProblem(); + int32_t contactIncrement = 0; + + auto contactSizeLocation = data_; + data_ = set(data_, contactIncrement); // reserving space + + /// contact position + for (auto *obj: world_->getObjList()) { + for (auto &contact: obj->getContacts()) { + if (!contact.isObjectA() && contact.getPairObjectBodyType()==BodyType::DYNAMIC) + continue; + + contactIncrement++; + // contact points + data_ = setInFloat(data_, contact.getPosition()); + + // contact forces + auto impulseB = contact.getImpulse(); + auto contactFrame = contact.getContactFrame(); + + Vec<3> impulseW; + raisim::matTransposevecmul(contactFrame, impulseB, impulseW); + impulseW /= world_->getTimeStep(); + data_ = setInFloat(data_, impulseW); } } + set(contactSizeLocation, contactIncrement); // object information - if(objectId_ > -1) { - RSFATAL_IF(objectId_ >= world_->getObjList().size(), "The client is requesting non-existent object") - auto *obSelected = world_->getObjList()[objectId_]; - if(obSelected->getObjectType() == ObjectType::ARTICULATED_SYSTEM) { + auto found = std::find_if(world_->getObjList().begin(), world_->getObjList().end(), + [this](const Object * ptr) { return ptr->visualTag == this->objectId_; }); + + if (found != world_->getObjList().end()) { + auto obSelected = *found; + if (obSelected->getObjectType() == ObjectType::ARTICULATED_SYSTEM) { data_ = set(data_, int32_t(1)); - auto* as = reinterpret_cast(obSelected); + auto *as = reinterpret_cast(obSelected); data_ = set(data_, int32_t(as->getGeneralizedCoordinateDim())); data_ = set(data_, int32_t(as->getDOF())); data_ = set(data_, int32_t(as->getMovableJointNames().size())); data_ = set(data_, int32_t(as->getFrames().size())); - for(int i=0; igetGeneralizedCoordinateDim(); i++) + for (int i = 0; i < as->getGeneralizedCoordinateDim(); i++) data_ = set(data_, float(as->getGeneralizedCoordinate()[i])); - for(int i=0; igetDOF(); i++) + for (int i = 0; i < as->getDOF(); i++) data_ = set(data_, float(as->getGeneralizedVelocity()[i])); - for(int i=0; igetMovableJointNames().size(); i++) { + for (int i = 0; i < as->getMovableJointNames().size(); i++) { data_ = set(data_, as->getMovableJointNames()[i]); int j = i; - if(as->getJointType(0) == Joint::Type::FIXED) + if (as->getJointType(0) == Joint::Type::FIXED) j = i + 1; Vec<3> vec; data_ = set(data_, int(as->getJointType(j))); - as->getPosition(j, {0,0,0}, vec); + as->getPosition(j, {0, 0, 0}, vec); data_ = setInFloat(data_, vec); vec = as->getJointAxis(j); data_ = setInFloat(data_, vec); @@ -1062,8 +1250,8 @@ class RaisimServer final { Vec<3> pos; Vec<4> quat; - Mat<3,3> rot; - for(int i=0; igetFrames().size(); i++) { + Mat<3, 3> rot; + for (int i = 0; i < as->getFrames().size(); i++) { data_ = set(data_, as->getFrames()[i].name); as->getFramePosition(i, pos); as->getFrameOrientation(i, rot); @@ -1072,7 +1260,7 @@ class RaisimServer final { } // COM position - if(as->getJointType(0) == Joint::FLOATING) { + if (as->getJointType(0) == Joint::FLOATING) { data_ = set(data_, int32_t(0)); data_ = setInFloat(data_, as->getCOM()); } else @@ -1080,30 +1268,48 @@ class RaisimServer final { } else { data_ = set(data_, int32_t(0)); - auto* sb = reinterpret_cast(obSelected); + auto *sb = reinterpret_cast(obSelected); + data_ = set(data_, int32_t(7)); + data_ = set(data_, int32_t(6)); + data_ = set(data_, int32_t(1)); + data_ = set(data_, int32_t(0)); auto pos = sb->getPosition(); auto quat = sb->getQuaternion(); data_ = setInFloat(data_, pos, quat, sb->getLinearVelocity(), sb->getAngularVelocity()); + data_ = set(data_, std::string("ROOT")); + data_ = set(data_, int(Joint::Type::FLOATING)); + data_ = setInFloat(data_, pos, pos); + data_ = set(data_, int32_t(0)); + data_ = setInFloat(data_, pos); } } else { data_ = set(data_, int32_t(-1)); } // charts - data_ = set(data_, (uint64_t) (charts_.size())); + data_ = set(data_, (int32_t) (charts_.size())); for (auto c: charts_) { - data_ = set(data_, int32_t(c.second->getType())); + bool initialized = c.second->visualTag != 0; + if (!initialized) c.second->visualTag = visTagCounter++; + + data_ = set(data_, (int32_t) c.second->getType(), initialized, c.second->visualTag); + if (!initialized) + data_ = c.second->initialize(data_); + data_ = c.second->serialize(data_); } } + inline bool receiveData(int seconds) { using namespace server; int totalDataSize = RECEIVE_BUFFER_SIZE, totalReceivedDataSize = 0, currentReceivedDataSize; while (totalDataSize > totalReceivedDataSize) { if (waitForMessageFromClient(seconds)) { - currentReceivedDataSize = recv(client_, &receive_buffer[0] + totalReceivedDataSize, RECEIVE_BUFFER_SIZE - totalReceivedDataSize, 0); + currentReceivedDataSize = + recv(client_, &receive_buffer[0] + totalReceivedDataSize, RECEIVE_BUFFER_SIZE - totalReceivedDataSize, 0); + if (currentReceivedDataSize == -1) return false; } else { RSWARN("Lost connection to the client. Trying to find a new client...") return false; @@ -1122,14 +1328,17 @@ class RaisimServer final { inline bool sendData() { using namespace server; - int dataSize = data_ - &send_buffer[0]; + int dataSize = int(data_ - &send_buffer[0]); int totalSentBytes = 0; + int currentlySentBytes = 0; set(&send_buffer[0], dataSize); while (dataSize > totalSentBytes) - if(waitForMessageToClient(1)) - totalSentBytes += send(client_, &send_buffer[0] + totalSentBytes, dataSize - totalSentBytes, 0); - else + if (waitForMessageToClient(1)) { + currentlySentBytes = send(client_, &send_buffer[0] + totalSentBytes, dataSize - totalSentBytes, 0); + totalSentBytes += currentlySentBytes; + if (currentlySentBytes == -1) return false; + } else return false; return true; @@ -1139,288 +1348,40 @@ class RaisimServer final { using namespace server; std::lock_guard guard(serverMutex_); ClientMessageType cMsgType; - int nASs; - rData_ = get(rData_, &cMsgType, &nASs); + int nSensors; + rData_ = get(rData_, &cMsgType, &nSensors); if (cMsgType != ClientMessageType::REQUEST_SENSOR_UPDATE) return false; - auto& obList = world_->getObjList(); - - for (int i=0; i < nASs; i++) { - int obIndex, nSensors; - rData_ = get(rData_, &obIndex, &nSensors); - auto* as = dynamic_cast(obList[obIndex]); - - RSFATAL_IF(as->getSensors().size() != nSensors, "updateSensorMeasurements: sensor size mismatch. This must be a bug. Please report") - for (int j=0; j < nSensors; j++) { - int needsUpdate; - rData_ = get(rData_, &needsUpdate); - if (needsUpdate == 0) continue; - - Sensor::Type type; - std::string name; - rData_ = get(rData_, &type, &name); - auto sensor = as->getSensors()[name]; - - if (type == Sensor::Type::RGB) { - int width, height; - rData_ = get(rData_, &width, &height); - - auto& img = std::static_pointer_cast(sensor)->getImageBuffer(); - RSFATAL_IF(width*height*4 != img.size(), "Image size mismatch. Sensor module not working properly") - rData_ = getN(rData_, img.data(), width * height * 4); - } else if (type == Sensor::Type::DEPTH) { - int width, height; - rData_ = get(rData_, &width, &height); - auto& depthArray = std::static_pointer_cast(sensor)->getDepthArray(); - RSFATAL_IF(width * height != depthArray.size(), "Image size mismatch. Sensor module not working properly") - rData_ = getN(rData_, depthArray.data(), width * height); - } - } - } - - return true; - } - - inline void serializeObjects() { - using namespace server; - auto &objList = world_->getObjList(); - - data_ = set(data_, ServerMessageType::INITIALIZATION); - data_ = set(data_, (uint64_t) world_->getConfigurationNumber() + visualConfiguration_); - data_ = set(data_, (uint64_t) (objList.size())); - - for (auto *ob : objList) { - // set name length - data_ = set(data_, (uint64_t) ob->getIndexInWorld(), ob->getObjectType(), ob->getName()); - - switch (ob->getObjectType()) { - case SPHERE: - data_ = set(data_, dynamic_cast(ob)->getAppearance()); - data_ = set(data_, float(dynamic_cast(ob)->getRadius())); - break; - - case BOX: - data_ = set(data_, dynamic_cast(ob)->getAppearance()); - for (int i = 0; i < 3; i++) - data_ = set(data_, float(dynamic_cast(ob)->getDim()[i])); - break; - - case CYLINDER: - data_ = set(data_, dynamic_cast(ob)->getAppearance()); - data_ = set(data_, float(dynamic_cast(ob)->getRadius())); - data_ = set(data_, float(dynamic_cast(ob)->getHeight())); - break; - - case CONE: - break; - - case CAPSULE: - data_ = set(data_, dynamic_cast(ob)->getAppearance()); - data_ = set(data_, float(dynamic_cast(ob)->getRadius())); - data_ = set(data_, float(dynamic_cast(ob)->getHeight())); - break; - - case HALFSPACE: - data_ = set(data_, dynamic_cast(ob)->getAppearance()); - data_ = set(data_, float(dynamic_cast(ob)->getHeight())); - break; - - case COMPOUND: - data_ = set(data_, dynamic_cast(ob)->getAppearance()); - data_ = set(data_, (uint64_t) (dynamic_cast(ob)->getObjList().size())); - - for (auto &vob : dynamic_cast(ob)->getObjList()) { - data_ = set(data_, vob.objectType); - data_ = set(data_, vob.appearance); - - switch (vob.objectType) { - case BOX: - data_ = set(data_, vob.objectParam[0], vob.objectParam[1], vob.objectParam[2]); - break; - case CAPSULE: - case CYLINDER: - data_ = set(data_, vob.objectParam[0], vob.objectParam[1]); - break; - case SPHERE: - data_ = set(data_, vob.objectParam[0]); - break; - - default: - break; - } - } - - break; - - case MESH: - data_ = set(data_, dynamic_cast(ob)->getAppearance()); - data_ = set(data_, dynamic_cast(ob)->getMeshFileName()); - data_ = set(data_, float(dynamic_cast(ob)->getScale())); - break; - - case HEIGHTMAP: - { - auto hm = dynamic_cast(ob); - data_ = set(data_, hm->getAppearance()); - data_ = setInFloat(data_, hm->getCenterX(), hm->getCenterY(), hm->getXSize(), hm->getYSize()); - data_ = set(data_, (uint64_t) hm->getXSamples(), (uint64_t) hm->getYSamples()); - data_ = setInFloat(data_, hm->getHeightVector()); - } - break; - - case ARTICULATED_SYSTEM: - { - auto as = dynamic_cast(ob); - data_ = set(data_, as->getResourceDir()); - - for (uint64_t i = 0; i < 2; i++) { - std::vector *visOb; - if (i == 0) - visOb = &as->getVisOb(); - else - visOb = &as->getVisColOb(); - - data_ = set(data_, (uint64_t) (visOb->size())); - - for (auto &vob : *visOb) { - data_ = set(data_, vob.shape, vob.material, vob.color, i); - if (vob.shape == Shape::Mesh) { - data_ = set(data_, vob.fileName, vob.scale); - } else { - data_ = set(data_, (uint64_t) (vob.visShapeParam.size())); - for (auto vparam : vob.visShapeParam) - data_ = set(data_, vparam); - } - } - } - - // add sensors - data_ = set(data_, (uint64_t) as->getSensors().size()); - for (auto& sensor: as->getSensors()) - data_ = sensor.second->serializeProp(data_); - } - break; - - case UNRECOGNIZED: - break; - } - } - - // constraints - data_ = set(data_, (uint64_t) (world_->getWires().size())); - - // charts - data_ = set(data_, (uint64_t) (charts_.size())); - for (auto c: charts_) { - data_ = set(data_, int32_t(c.second->getType())); - data_ = c.second->initialize(data_); - } - } - - inline void serializeContacts() { - using namespace server; - auto *contactList = world_->getContactProblem(); - - // set message type - data_ = set(data_, ServerMessageType::CONTACT_INFO_UPDATE); - - // Data begins - size_t contactSize = contactList->size(); - for (const auto &con : *contactList) - if (con.rank == 5) contactSize--; - - size_t contactIncrement = 0; - auto contactSizeLocation = data_; - - data_ = set(data_, (uint64_t) (contactSize)); - - for (auto *obj : world_->getObjList()) { - for (auto &contact : obj->getContacts()) { - if (!contact.isObjectA() && - contact.getPairObjectBodyType() != raisim::BodyType::STATIC) - continue; - contactIncrement++; - // contact points - auto contactPos = contact.getPosition(); - data_ = set(data_, contactPos); - - // contact forces - auto impulseB = contact.getImpulse(); - auto contactFrame = contact.getContactFrame(); - - Vec<3> impulseW; - raisim::matTransposevecmul(contactFrame, impulseB, impulseW); - impulseW /= world_->getTimeStep(); - data_ = set(data_, impulseW); - } - } - set(contactSizeLocation, contactIncrement); - } - - inline void serializeVisuals() { - using namespace server; - data_ = set(data_, (uint64_t) (visuals_.size())); - data_ = set(data_, (uint64_t) (instancedvisuals_.size())); - data_ = set(data_, (uint64_t) (visualAs_.size())); - - for (auto &kAndVo : visuals_) { - auto &vo = kAndVo.second; - data_ = set(data_, vo->type, vo->name, vo->color, vo->material, vo->glow, vo->shadow); - - switch (vo->type) { - case Visuals::VisualSphere: - data_ = set(data_, (float) vo->size[0]); - break; - - case Visuals::VisualBox: - data_ = setInFloat(data_, vo->size); - break; - - case Visuals::VisualMesh: - data_ = setInFloat(data_, vo->size); - data_ = set(data_, vo->meshFileName); - break; - - case Visuals::VisualCylinder: - case Visuals::VisualCapsule: - case Visuals::VisualArrow: - data_ = set(data_, (float) vo->size[0], (float) vo->size[1]); - break; - - default: - break; + auto &obList = world_->getObjList(); + + for (int i = 0; i < nSensors; i++) { + uint32_t visualTag; + std::string name; + Sensor::Type type; + rData_ = get(rData_, &visualTag, &type, &name); + + ArticulatedSystem *as = dynamic_cast(*std::find_if(obList.begin(), obList.end(), + [visualTag](const Object* i){ return i->visualTag == visualTag; })); + auto sensor = as->getSensors()[name]; + + if (type == Sensor::Type::RGB) { + int width, height; + rData_ = get(rData_, &width, &height); + auto &img = std::static_pointer_cast(sensor)->getImageBuffer(); + RSFATAL_IF(width * height * 4 != img.size(), "Image size mismatch. Sensor module not working properly") + rData_ = getN(rData_, img.data(), width * height * 4); + } else if (type == Sensor::Type::DEPTH) { + int width, height; + rData_ = get(rData_, &width, &height); + auto &depthArray = std::static_pointer_cast(sensor)->getDepthArray(); + RSFATAL_IF(width * height != depthArray.size(), "Image size mismatch. Sensor module not working properly") + rData_ = getN(rData_, depthArray.data(), width * height); } - } - for (auto &iv : instancedvisuals_) { - auto v = iv.second; - data_ = set(data_, v->name, v->type); - data_ = setInFloat(data_, v->size, v->color1, v->color2); } - for (auto &vas : visualAs_) { - auto *ob = &vas.second->obj; - data_ = set(data_, vas.first); - std::string resDir = static_cast(ob)->getResourceDir(); - data_ = set(data_, resDir); - - for (uint64_t i = 0; i < 2; i++) { - std::vector *visOb = i==0 ? &(ob->getVisOb()) : &(ob->getVisColOb()); - data_ = set(data_, (uint64_t) (visOb->size())); - - for (auto &vob : *visOb) { - data_ = set(data_, vob.shape, vob.material, vas.second->color, i); - if (vob.shape == Shape::Mesh) { - data_ = set(data_, vob.fileName, vob.scale); - } else { - data_ = set(data_, (uint64_t) (vob.visShapeParam.size())); - for (auto vparam : vob.visShapeParam) - data_ = set(data_, vparam); - } - } - } - } + return true; } char *data_, *rData_; @@ -1431,9 +1392,9 @@ class RaisimServer final { char tempBuffer[MAXIMUM_PACKET_SIZE]; int state_ = STATUS_RENDERING; std::vector serverRequest_; - std::string focusedObjectName_; + uint32_t focusedObjectVisTag_; std::string videoName_; - int objectId_; + uint32_t objectId_; std::atomic terminateRequested_ = {false}; int client_; int server_fd_; @@ -1441,6 +1402,7 @@ class RaisimServer final { int addrlen; std::thread serverThread_; std::future threadResult_; + std::string mapName_; std::mutex serverMutex_; @@ -1452,7 +1414,10 @@ class RaisimServer final { int screenShotWidth_, screenShotHeight_; // version - constexpr static int version_ = 10007; + constexpr static int version_ = 10009; + + // visual tag counter + uint32_t visTagCounter = 30; std::unordered_map visuals_; std::unordered_map instancedvisuals_; @@ -1461,14 +1426,29 @@ class RaisimServer final { std::map charts_; public: - inline TimeSeriesGraph* addTimeSeriesGraph(std::string title, std::vector names, std::string xAxis, std::string yAxis) { + /** + * Only works with RaisimUnreal. Please read the "atlas" example to see how it works. + * @param[in] title title of the chart + * @param[in] names name of the data curves to be plotted + * @param[in] xAxis title of the x-axis + * @param[in] yAxis title of the y-axis + * @return pointer to the created Time Series Graph */ + inline TimeSeriesGraph *addTimeSeriesGraph(std::string title, + std::vector names, + std::string xAxis, + std::string yAxis) { RSFATAL_IF(charts_.find(title) != charts_.end(), "A chart named " << title << "already exists") auto chart = new TimeSeriesGraph(std::ref(title), std::ref(names), std::ref(xAxis), std::ref(yAxis)); charts_[title] = chart; return chart; } - inline BarChart* addBarChart(std::string title, std::vector names) { + /** + * Only works with RaisimUnreal. Please read the "atlas" example to see how it works. + * @param[in] title title of the chart + * @param[in] names name of the data histogram to be plotted + * @return pointer to the created Bar Chart */ + inline BarChart *addBarChart(std::string title, std::vector names) { RSFATAL_IF(charts_.find(title) != charts_.end(), "A chart named " << title << "already exists") auto chart = new BarChart(std::ref(title), std::ref(names)); charts_[title] = chart; diff --git a/raisim/m1/include/raisim/Terrain.hpp b/raisim/m1/include/raisim/Terrain.hpp index b50f6b75..ca63694e 100644 --- a/raisim/m1/include/raisim/Terrain.hpp +++ b/raisim/m1/include/raisim/Terrain.hpp @@ -107,6 +107,7 @@ struct TerrainProperties { double fractalLacunarity, double fractalGain, double stepSize, + double heightOffset, std::uint32_t seed) : frequency(frequency), xSize(xSize), @@ -118,7 +119,8 @@ struct TerrainProperties { fractalLacunarity(fractalLacunarity), fractalGain(fractalGain), stepSize(stepSize), - seed(seed) { + seed(seed), + heightOffset(heightOffset){ }; double frequency = 0.1; @@ -128,6 +130,7 @@ struct TerrainProperties { double fractalLacunarity = 2.0; double fractalGain = 0.5; double stepSize = 0; + double heightOffset = 0; size_t fractalOctaves = 5; size_t xSamples = 100; @@ -187,6 +190,10 @@ class TerrainGenerator { height_[y * terrainProperties_.xSamples + x] = e; } + for (int y = 0; y < terrainProperties_.ySamples; y++) + for (int x = 0; x < terrainProperties_.xSamples; x++) + height_[y * terrainProperties_.xSamples + x] += terrainProperties_.heightOffset; + return height_; } diff --git a/raisim/m1/include/raisim/constraints/Constraints.hpp b/raisim/m1/include/raisim/constraints/Constraints.hpp index 7c35c4b2..8cf8b30a 100644 --- a/raisim/m1/include/raisim/constraints/Constraints.hpp +++ b/raisim/m1/include/raisim/constraints/Constraints.hpp @@ -10,11 +10,18 @@ namespace raisim { class Constraints { + friend class raisim::RaisimServer; + public: + Constraints() { color_ = {1.0, 1.0, 1.0, 1.0}; } virtual ~Constraints() = default; + [[nodiscard]] const Vec<4>& getColor() const { return color_; }; + void setColor(const Vec<4>& color) { color_ = color; }; protected: std::string name_; + Vec<4> color_; + uint32_t visualTag = 0; }; } diff --git a/raisim/m1/include/raisim/contact/Contact.hpp b/raisim/m1/include/raisim/contact/Contact.hpp index 76a23612..a09f55b6 100644 --- a/raisim/m1/include/raisim/contact/Contact.hpp +++ b/raisim/m1/include/raisim/contact/Contact.hpp @@ -12,15 +12,21 @@ #include #include -namespace raisim { +namespace raisim { class World; class Object; +class SingleBodyObject; +class ArticulatedSystem; +} +namespace raisim { class Contact { public: - friend class raisim::World; + friend class raisim::ArticulatedSystem; + friend class raisim::Object; + friend class raisim::SingleBodyObject; Contact() = default; @@ -81,68 +87,140 @@ class Contact { frame[8] = zAxis[2] * zNormInv; } - const Vec<3> &getPosition() const { +public: + [[nodiscard]] const Vec<3> &getPosition() const { return position_; } - const Vec<3> &getNormal() const { + [[nodiscard]] const Vec<3> &getNormal() const { return normal_; } - const Mat<3, 3> &getContactFrame() const { + /** + * treturns a TRANSPOSE of the frame that the impulse is expressed. + * @return contact frame + */ + [[nodiscard]] const Mat<3, 3> &getContactFrame() const { return frame_; } - size_t getIndexContactProblem() const { + /** + * returns the corresponding index in raisim::World::getContactProblem + * @return contact index + */ + [[nodiscard]] size_t getIndexContactProblem() const { return contactProblemIndex_; } - size_t getIndexInObjectContactList() const { + /** + * returns the corresponding index in raisim::Object::getContacts + * @return contact index + */ + [[nodiscard]] size_t getIndexInObjectContactList() const { return contactIndexInObject_; } - size_t getPairObjectIndex() const { + /** + * returns the contacting object index in raisim::World::getObjectList + * @return object index + */ + [[nodiscard]] size_t getPairObjectIndex() const { return pairObjectIndex_; } - size_t getPairContactIndexInPairObject() const { + /** + * returns the contact index in the contacting (the paired) object in raisim::Object::getContacts + * @return contact index + */ + [[nodiscard]] size_t getPairContactIndexInPairObject() const { return pairContactIndexInPairObject_; } - const Vec<3> &getImpulse() const { + /** + * returns the impulse. You have to multiply this number by the time step to get the force + * @return impulse + */ + [[nodiscard]] const Vec<3> &getImpulse() const { return *impulse_; } - bool isObjectA() const { + /** + * returns if the object is objectA. When there is a contact, the two paired objects are assigned to be objectA and objectB arbitrarily. + * The contact frame is defined such that its z-axis is pointing towards the objectA. + * The contact impulse is defined as an external force that objectA experiences. + * @return impulse + */ + [[nodiscard]] bool isObjectA() const { return objectA_; } - BodyType getPairObjectBodyType() const { + /** + * returns the body type of the paired object. Please read https://raisim.com/sections/Object.html#body-types to learn about it + * @return the body type + */ + [[nodiscard]] BodyType getPairObjectBodyType() const { return pairObjectBodyType_; } - Mat<3, 3> &getInvInertia() { + /** + * returns the inverse apparent inertia of the contacting point + * @return inertia + */ + [[nodiscard]] Mat<3, 3> &getInvInertia() { return Minv_; } - size_t getlocalBodyIndex() const { + /** + * get local body index of this contact. The local index is assigned to each moving body in an object + * @return the body index. + */ + [[nodiscard]] size_t getlocalBodyIndex() const { return localBodyIndex_; } - double getDepth() const { + /** + * The penetration depth of the contact + * @return the depth of the contact + */ + [[nodiscard]] double getDepth() const { return depth_; } - bool isSelfCollision() const { + /** + * returns if the contact is self collision + * @return if this is a self collision + */ + [[nodiscard]] bool isSelfCollision() const { return isSelfCollision_; } - void setSelfCollision() { - isSelfCollision_ = true; + /** + * this is set true for one self-collision point so that you don't count them twice + * @return if you can skip this contact point while iterating contact points + */ + [[nodiscard]] bool skip() const { + return skip_; } - bool skip() const { - return skip_; + /** + * get the collision body of this object. This is ODE interface + * @return collision body + */ + [[nodiscard]] dGeomID getCollisionBodyA() { + return colA_; + } + + /** + * get the collision body of the pairing object. This is ODE interface + * @return collision body + */ + [[nodiscard]] dGeomID getCollisionBodyB() { + return colB_; + } + +protected: + void setSelfCollision() { + isSelfCollision_ = true; } void setSkip() { @@ -157,13 +235,7 @@ class Contact { impulse_ = im; } - dGeomID getCollisionBodyA() { - return colA_; - } - dGeomID getCollisionBodyB() { - return colB_; - } private: Mat<3, 3> frame_; // contactFrame of A diff --git a/raisim/m1/include/raisim/math/Matrix.hpp b/raisim/m1/include/raisim/math/Matrix.hpp index a867fe7b..e0e53131 100644 --- a/raisim/m1/include/raisim/math/Matrix.hpp +++ b/raisim/m1/include/raisim/math/Matrix.hpp @@ -78,6 +78,12 @@ class Mat : public MatExpr> { inline double operator()(size_t i, size_t j) const { return v[i + j * n]; } inline double &operator()(size_t i, size_t j) { return v[i + j * n]; } + inline Mat operator-() { + Mat result = *this; + result *=-1; + return result; + } + RAIMATH_MATEXPR_OPERATORS EigenMat e() { return EigenMat(v);} diff --git a/raisim/m1/include/raisim/object/ArticulatedSystem/ArticulatedSystem.hpp b/raisim/m1/include/raisim/object/ArticulatedSystem/ArticulatedSystem.hpp index 91144f42..5676f47c 100644 --- a/raisim/m1/include/raisim/object/ArticulatedSystem/ArticulatedSystem.hpp +++ b/raisim/m1/include/raisim/object/ArticulatedSystem/ArticulatedSystem.hpp @@ -1306,7 +1306,7 @@ class ArticulatedSystem : public Object { * For a sphere, {raidus}. * For a cylinder and a capsule, {radius, length} * For a box, {x-dim, y-dim, z-dim} */ - void setCollisionObjectShapeParameters(size_t id, const std::vector ¶ms); + void setCollisionObjectShapeParameters(size_t id, const Vec<4> ¶ms); /** * change collision geom offset from the joint position. diff --git a/raisim/m1/include/raisim/object/ArticulatedSystem/JointAndBodies.hpp b/raisim/m1/include/raisim/object/ArticulatedSystem/JointAndBodies.hpp index 984dd5fd..2dce1e08 100644 --- a/raisim/m1/include/raisim/object/ArticulatedSystem/JointAndBodies.hpp +++ b/raisim/m1/include/raisim/object/ArticulatedSystem/JointAndBodies.hpp @@ -25,8 +25,7 @@ class LoadFromURDF2; /* shapes that raisim supports */ namespace Shape { -enum Type : - int { +enum Type : int { Box = 0, Cylinder, Sphere, @@ -34,6 +33,12 @@ enum Type : Capsule, Cone, // cone is not currently supported Ground, + CoordinateFrame, + Arrow, + RotationalArrow, + HeightMap, + PolyLine, + SingleLine, NotShape }; @@ -111,7 +116,7 @@ struct VisObject { * vis_name: name of the visualized body, * vis_material: material */ VisObject(Shape::Type vis_shape, - const std::vector &vis_shapeParam, + const raisim::Vec<4> &vis_shapeParam, const raisim::Vec<3> &vis_origin, const raisim::Mat<3, 3> &vis_rotMat, const raisim::Vec<4> &vis_color, @@ -155,7 +160,7 @@ struct VisObject { * vis_name: name of the visualized body, * vis_material: material */ VisObject(Shape::Type vis_shape, - const std::vector &vis_shapeParam, + const raisim::Vec<4> &vis_shapeParam, const raisim::Vec<3> &vis_origin, const raisim::Mat<3, 3> &vis_rotMat, const raisim::Vec<4> &vis_color, @@ -169,7 +174,7 @@ struct VisObject { } Shape::Type shape; - std::vector visShapeParam; + raisim::Vec<4> visShapeParam; raisim::Vec<3> offset; raisim::Mat<3, 3> rot; raisim::Vec<4> color; @@ -303,7 +308,7 @@ struct CollisionBody { * col_material: collision material that defines contact physics * col_visualizedMaterial: how the collision body should be visualized */ CollisionBody(Shape::Type col_shape, - const std::vector &col_shapeParam, + const Vec<4> &col_shapeParam, const raisim::Vec<3> &col_origin, const raisim::Mat<3, 3> &col_rotMat, const std::string &col_name, @@ -336,7 +341,7 @@ struct CollisionBody { Shape::Type shape; raisim::Vec<3> scale; - std::vector shapeParam; + raisim::Vec<4> shapeParam; raisim::Vec<3> offset; raisim::Mat<3, 3> rot; std::string name; @@ -360,7 +365,7 @@ struct CollisionBody { * col_visualizedMaterial: how the collision body should be visualized * col_meshFile: file location relative to the resource directory of the articulated system */ CollisionBody(Shape::Type col_shape, - const std::vector &col_shapeParam, + const Vec<4> &col_shapeParam, const raisim::Vec<3> &col_origin, const raisim::Mat<3, 3> &col_rotMat, const raisim::Vec<3> &col_meshScale, @@ -375,7 +380,7 @@ struct CollisionBody { }; inline void getInertialAssumingUniformDensity(Shape::Type shape, - const std::vector &shapeParam, + const Vec<4> &shapeParam, const Mat<3,3>& rot, double density, double &mass, @@ -519,7 +524,7 @@ class Body { void addCollisionObject(Shape::Type shape, - const std::vector ¶m, + const Vec<4> ¶m, const raisim::Vec<3> &origin, const raisim::Mat<3, 3> &rot, const raisim::Vec<3> &scale, @@ -541,7 +546,7 @@ class Body { } void addVisualObject(Shape::Type shape, - const std::vector &shapeParam, + const Vec<4> &shapeParam, const raisim::Vec<3> &origin, const raisim::Mat<3, 3> &rotMat, const raisim::Vec<4> &color, diff --git a/raisim/m1/include/raisim/object/ArticulatedSystem/loaders.hpp b/raisim/m1/include/raisim/object/ArticulatedSystem/loaders.hpp index 0aba82cc..8dd3960f 100644 --- a/raisim/m1/include/raisim/object/ArticulatedSystem/loaders.hpp +++ b/raisim/m1/include/raisim/object/ArticulatedSystem/loaders.hpp @@ -54,7 +54,7 @@ struct UrdfBody { Vec<3> origin; Mat<3, 3> rot; Vec<3> scale; - std::vector param; + Vec<4> param; raisim::Vec<4> color; std::string mat; std::string collision_mat; @@ -187,12 +187,12 @@ class LoadFromMjcf { const RaiSimTinyXmlWrapper &node, Shape::Type type, const std::string &typeName, - std::vector ¶m, + Vec<4> ¶m, Mat<3, 3> &rot, Vec<3> &pos, const MjcfCompilerSetting& setting); - static void getMjcfSizeParam(const RaiSimTinyXmlWrapper &g, Shape::Type type, std::vector ¶m); + static void getMjcfSizeParam(const RaiSimTinyXmlWrapper &g, Shape::Type type, Vec<4> ¶m); static void getMjcfPos(const RaiSimTinyXmlWrapper &g, Vec<3> &pos); static void posFromFromTo(const RaiSimTinyXmlWrapper &g, Vec<3> &pos); static void getMjcfOrientation(const RaiSimTinyXmlWrapper &g, Mat<3, 3> &rot, const std::string& eulerseq, const std::string& anglerep); diff --git a/raisim/m1/include/raisim/object/Object.hpp b/raisim/m1/include/raisim/object/Object.hpp index 829992d5..56b7c077 100644 --- a/raisim/m1/include/raisim/object/Object.hpp +++ b/raisim/m1/include/raisim/object/Object.hpp @@ -20,6 +20,7 @@ namespace raisim { class World; +class RaisimServer; namespace contact { class BisectionContactSolver; @@ -28,6 +29,7 @@ class Single3DContactProblem; class Object { friend class raisim::World; + friend class raisim::RaisimServer; friend class raisim::contact::BisectionContactSolver; friend class raisim::contact::Single3DContactProblem; @@ -44,13 +46,13 @@ class Object { * get the world index. raisim::World::getObjects() returns a vector of object pointers. * This is method returns the index of this object in the vector. * @return the world index */ - size_t getIndexInWorld() const; + [[nodiscard]] size_t getIndexInWorld() const; /** * get a vector of all contacts on the object. * @return contacts on the body */ - const std::vector &getContacts() const; - std::vector &getContacts(); + [[nodiscard]] const std::vector &getContacts() const; + [[nodiscard]] std::vector &getContacts(); virtual void updateCollision() = 0; virtual void preContactSolverUpdate1(const Vec<3> &gravity, double dt) = 0; @@ -67,13 +69,13 @@ class Object { // spring force is not visualized virtual void setConstraintForce(size_t localIdx, const Vec<3>& pos, const Vec<3>& force) = 0; - virtual double getMass(size_t localIdx) const = 0; + [[nodiscard]] virtual double getMass(size_t localIdx) const = 0; /** * get the object type. * Possible types are SPHERE, BOX, CYLINDER, CONE, CAPSULE, MESH, HALFSPACE, COMPOUND, HEIGHTMAP, ARTICULATED_SYSTEM * @return the object type */ - virtual ObjectType getObjectType() const = 0; + [[nodiscard]] virtual ObjectType getObjectType() const = 0; virtual void getPosition(size_t localIdx, Vec<3>& pos_w) const = 0; virtual void getVelocity(size_t localIdx, Vec<3>& vel_w) const = 0; virtual void getOrientation(size_t localIdx, Mat<3,3>& rot) const = 0; @@ -83,13 +85,13 @@ class Object { * get the object body type. * Available types are: DYNAMIC (movable and finite mass), STATIC (not movable and infinite mass), KINETIC (movable and infinite mass) * @return the body type */ - virtual BodyType getBodyType(size_t localIdx) const { return bodyType_; }; + [[nodiscard]] virtual BodyType getBodyType(size_t localIdx) const { return bodyType_; }; /** * get the object body type. * Available types are: DYNAMIC (movable and finite mass), STATIC (not movable and infinite mass), KINETIC (movable and infinite mass). * @return the body type */ - virtual BodyType getBodyType() const { return bodyType_; }; + [[nodiscard]] virtual BodyType getBodyType() const { return bodyType_; }; /** * get the contact point velocity in the world frame. @@ -105,13 +107,13 @@ class Object { /** * get the name of the object * @return name of the object */ - const std::string& getName() const { return name_; } + [[nodiscard]] const std::string& getName() const { return name_; } // external force visualization - const std::vector>& getExternalForce() const { return externalForceViz_; } - const std::vector>& getExternalForcePosition() const { return externalForceVizPos_; } - const std::vector>& getExternalTorque() const { return externalTorqueViz_; } - const std::vector>& getExternalTorquePosition() const { return externalTorqueVizPos_; } + [[nodiscard]] const std::vector>& getExternalForce() const { return externalForceViz_; } + [[nodiscard]] const std::vector>& getExternalForcePosition() const { return externalForceVizPos_; } + [[nodiscard]] const std::vector>& getExternalTorque() const { return externalTorqueViz_; } + [[nodiscard]] const std::vector>& getExternalTorquePosition() const { return externalTorqueVizPos_; } virtual void clearExternalForcesAndTorques() = 0; protected: @@ -146,6 +148,9 @@ class Object { std::vector> externalTorqueVizPos_; std::vector constraintJaco_; std::vector> constraintForce_; + + protected: + uint32_t visualTag = 0; }; } // raisim diff --git a/raisim/m1/include/raisim/object/singleBodies/Compound.hpp b/raisim/m1/include/raisim/object/singleBodies/Compound.hpp index b41d862b..2de7deb7 100644 --- a/raisim/m1/include/raisim/object/singleBodies/Compound.hpp +++ b/raisim/m1/include/raisim/object/singleBodies/Compound.hpp @@ -18,7 +18,7 @@ class Compound : public SingleBodyObject { public: struct CompoundObjectChild { - std::string childCompound; + std::string childCompound; // not used currently ObjectType objectType; Vec<4> objectParam; std::string material; diff --git a/raisim/m1/include/raisim/object/singleBodies/SingleBodyObject.hpp b/raisim/m1/include/raisim/object/singleBodies/SingleBodyObject.hpp index 64dde3b6..24f12d89 100644 --- a/raisim/m1/include/raisim/object/singleBodies/SingleBodyObject.hpp +++ b/raisim/m1/include/raisim/object/singleBodies/SingleBodyObject.hpp @@ -29,7 +29,7 @@ class SingleBodyObject : public Object { /** * returns the quaternion in Eigen::Vector4d * @return the orientation of the object */ - inline Eigen::Vector4d getQuaternion() const { + [[nodiscard]] inline Eigen::Vector4d getQuaternion() const { Eigen::Vector4d quat = bodyQuaternion_.e(); return quat; }; @@ -44,7 +44,7 @@ class SingleBodyObject : public Object { /** * returns the rotation matrix in Eigen::Matrix3d * @return the orientation of the object */ - inline Eigen::Matrix3d getRotationMatrix() const { + [[nodiscard]] inline Eigen::Matrix3d getRotationMatrix() const { Eigen::Matrix3d rot = bodyRotationMatrix_.e(); return rot; }; @@ -61,7 +61,7 @@ class SingleBodyObject : public Object { * returns the body position in Eigen::Vector3d. * Currently, all body positions are the same as the COM position * @return the position of the object */ - inline Eigen::Vector3d getPosition() const { + [[nodiscard]] inline Eigen::Vector3d getPosition() const { Eigen::Vector3d pos = bodyPosition_.e(); return pos; }; @@ -70,7 +70,7 @@ class SingleBodyObject : public Object { * returns the body position in Eigen::Vector3d. * Currently, all body positions are the same as the COM position * @return the position of the object */ - inline Eigen::Vector3d getComPosition() const { + [[nodiscard]] inline Eigen::Vector3d getComPosition() const { Eigen::Vector3d pos = comPosition_.e(); return pos; }; @@ -79,7 +79,7 @@ class SingleBodyObject : public Object { * returns the body position in raisim::Vec<3>. * Currently, all body positions are the same as the COM position * @return the position of the object */ - inline const raisim::Vec<3>& getComPosition_rs() const { + [[nodiscard]] inline const raisim::Vec<3>& getComPosition_rs() const { return comPosition_; }; @@ -87,14 +87,14 @@ class SingleBodyObject : public Object { * returns the body position in raisim::Vec<3>. * Currently, all body positions are the same as the COM position * @return the position of the object */ - inline const raisim::Vec<3>& getBodyToComPosition_rs() const { + [[nodiscard]] inline const raisim::Vec<3>& getBodyToComPosition_rs() const { return body2com_; }; /** * returns the linear velocity * @return the linear velocity of the object */ - inline Eigen::Vector3d getLinearVelocity() const { + [[nodiscard]] inline Eigen::Vector3d getLinearVelocity() const { Eigen::Vector3d vel = linVel_.e(); return vel; }; @@ -107,7 +107,7 @@ class SingleBodyObject : public Object { /** * returns the angular velocity * @return the angular velocity of the object */ - inline Eigen::Vector3d getAngularVelocity() const { + [[nodiscard]] inline Eigen::Vector3d getAngularVelocity() const { Eigen::Vector3d vel = angVel_.e(); return vel; }; @@ -117,10 +117,19 @@ class SingleBodyObject : public Object { * @param[out] angVel the angular velocity of the object */ inline void getAngularVelocity(Vec<3>& angVel) { angVel = angVel_; } + /** + * returns position vector. + * @param[in] localIdx this should be always 0 (this method is just to keep the same api as the ArticulatedSystem class) + * @param[out] pos_w the position vector of the object */ inline void getPosition(size_t localIdx, Vec<3>& pos_w) const final { pos_w = bodyPosition_; } + + /** + * returns the orientation of the object + * @param[in] localIdx local idx should be always 0 (this method is just to keep the same api as the ArticulatedSystem class) + * @param[out] pos_w the rotation matrix of the object */ inline void getOrientation(size_t localIdx, Mat<3,3>& rot) const final { rot = bodyRotationMatrix_; } @@ -128,29 +137,29 @@ class SingleBodyObject : public Object { /** * returns the kinetic energy * @return the kinetic energy of the object */ - double getKineticEnergy() const; + [[nodiscard]] double getKineticEnergy() const; /** * returns the potential energy w.r.t. z=0 and the given gravitational acceleration - * param[in] gravity gravitational acceleration + * @param[in] gravity gravitational acceleration * @return the potential energy of the object */ - double getPotentialEnergy(const Vec<3> &gravity) const; + [[nodiscard]] double getPotentialEnergy(const Vec<3> &gravity) const; /** * equivalent to getKineticEnergy() + getPotentialEnergy(gravity) - * param[in] gravity gravitational acceleration + * @param[in] gravity gravitational acceleration * @return the sum of the potential and gravitational energy of the object */ - double getEnergy(const Vec<3> &gravity) const; + [[nodiscard]] double getEnergy(const Vec<3> &gravity) const; /** * returns the linear momentum of the object * @return the linear momentum of the object */ - Eigen::Vector3d getLinearMomentum() const; + [[nodiscard]] Eigen::Vector3d getLinearMomentum() const; /** * returns the mass of the object. The localIdx is unused * @return the linear momentum of the object */ - double getMass(size_t localIdx = 0) const override; + [[nodiscard]] double getMass(size_t localIdx = 0) const override; /** * set the mass of the object. @@ -163,7 +172,7 @@ class SingleBodyObject : public Object { * get the inertia matrix in the body frame. * This value is constant. * @return the inertia matrix in the body frame */ - inline Eigen::Matrix3d getInertiaMatrix_B() const { + [[nodiscard]] inline Eigen::Matrix3d getInertiaMatrix_B() const { Eigen::Matrix3d iner = inertia_b_.e(); return iner; } @@ -172,7 +181,7 @@ class SingleBodyObject : public Object { * get the inertia matrix in the world frame. * This value changes as the body rotates. * @return the inertia matrix in the world frame */ - inline Eigen::Matrix3d getInertiaMatrix_W() const { + [[nodiscard]] inline Eigen::Matrix3d getInertiaMatrix_W() const { Eigen::Matrix3d iner = inertia_w_.e(); return iner; } @@ -181,7 +190,7 @@ class SingleBodyObject : public Object { * get the inertia matrix in the body frame (raisim matrix type). * This value is constant. * @return the inertia matrix in the body frame */ - const raisim::Mat<3,3>& getInertiaMatrix_B_rs() const { + [[nodiscard]] const raisim::Mat<3,3>& getInertiaMatrix_B_rs() const { return inertia_b_; } @@ -189,17 +198,39 @@ class SingleBodyObject : public Object { * get the inertia matrix in the world frame (raisim matrix type). * This value changes as the body rotates. * @return the inertia matrix in the world frame */ - const raisim::Mat<3,3>& getInertiaMatrix_W_rs() const { + [[nodiscard]] const raisim::Mat<3,3>& getInertiaMatrix_W_rs() const { return inertia_w_; } - ObjectType getObjectType() const final; - dGeomID getCollisionObject() const; + [[nodiscard]] ObjectType getObjectType() const final; + [[nodiscard]] dGeomID getCollisionObject() const; + + [[nodiscard]] GyroscopicMode getGyroscopicMode() const; - GyroscopicMode getGyroscopicMode() const; + /** + * Set position of the object (using Eigen) + * @param[in] originPosition Position + */ virtual void setPosition(const Eigen::Vector3d &originPosition); + + /** + * Set position of the object (using three doubles) + * @param[in] x x position + * @param[in] y y position + * @param[in] z z position + */ virtual void setPosition(double x, double y, double z); + + /** + * Set position of the object (using raisim::Vec<3>) + * @param[in] originPosition Position + */ virtual void setPosition(const Vec<3>& pos); + + /** + * Set orientation of the object (using Eigen::Quaterniond) + * @param[in] quaternion quaternion + */ virtual void setOrientation(const Eigen::Quaterniond &quaternion) { bodyQuaternion_ = {quaternion.w(), quaternion.x(), @@ -208,58 +239,177 @@ class SingleBodyObject : public Object { bodyQuaternion_ /= bodyQuaternion_.norm(); updateOrientation(); } + + /** + * Set orientation of the object (using Eigen::Vector4d) + * @param[in] quaternion quaternion + */ virtual void setOrientation(const Eigen::Vector4d &quaternion) { bodyQuaternion_.e() = quaternion; updateOrientation(); } + + /** + * Set orientation of the object (using doubles) + * @param[in] w w + * @param[in] x w + * @param[in] y w + * @param[in] z w + */ virtual void setOrientation(double w, double x, double y, double z) { bodyQuaternion_ = {w, x, y, z}; updateOrientation(); } + + /** + * Set orientation of the object (using Eigen::Matrix3d) + * @param[in] rotationMatrix rotation matrix + */ virtual void setOrientation(const Eigen::Matrix3d &rotationMatrix) { Eigen::Quaterniond quaternion(rotationMatrix); setOrientation(quaternion); } + + /** + * Set orientation of the object (using Vec<4>) + * @param[in] quat quaternion + */ virtual void setOrientation(const Vec<4>& quat) { bodyQuaternion_ = quat; updateOrientation(); } + /** + * Set both the position and orientation of the object (using Eigen::Vector3d and Eigen::Quaterniond) + * @param[in] originPosition position + * @param[in] quaternion quaternion + */ virtual void setPose(const Eigen::Vector3d &originPosition, const Eigen::Quaterniond &quaternion); + + /** + * Set both the position and orientation of the object (using Eigen::Vector3d and Eigen::Vector4d) + * @param[in] originPosition position + * @param[in] quaternion quaternion + */ virtual void setPose(const Eigen::Vector3d &originPosition, const Eigen::Vector4d &quaternion); + + /** + * Set both the position and orientation of the object (using Eigen::Vector3d and Eigen::Matrix3d) + * @param[in] originPosition position + * @param[in] rotationMatrix rotation matrix + */ virtual void setPose(const Eigen::Vector3d &originPosition, const Eigen::Matrix3d &rotationMatrix); + + + /** + * Set inertia of the object (using Eigen::Matrix3d) + * @param[in] inertia inertia of the object + */ void setInertia(const Eigen::Matrix3d& inertia); + + /** + * Set inertia of the object (using raisim::Mat<3,3>) + * @param[in] inertia inertia of the object + */ void setInertia(const Mat<3,3>& inertia) { inertia_b_ = inertia; cholInv<3>(inertia_b_.ptr(), inverseInertia_b_.ptr()); updateWorldInertia(); } + /** + * get the center of mass position + * @return the position of the center of mass + */ const Vec<3>& getCom() { return body2com_; } + + /** + * set the center of mass position + * @param[in] com the position of the center of mass + */ void setCom(const Vec<3>& com) { body2com_ = com; } + /** + * set both the linear and angular velocity of the object (using Eigen::Vector3d) + * @param[in] linearVelocity the linear velocity of the object + * @param[in] angularVelocity the angular velocity of the object + */ virtual void setVelocity(const Eigen::Vector3d &linearVelocity, const Eigen::Vector3d &angularVelocity) { linVel_.e() = linearVelocity; angVel_.e() = angularVelocity; } - inline void setVelocity(const Vec<3>& linVel, const Vec<3>& angVel) { linVel_ = linVel; angVel_ = angVel; } - + /** + * set both the linear and angular velocity of the object (using raisim::Vec<3>) + * @param[in] linearVelocity the linear velocity of the object + * @param[in] angularVelocity the angular velocity of the object + */ + inline void setVelocity(const Vec<3>& linearVelocity, const Vec<3>& angularVelocity) { linVel_ = linearVelocity; angVel_ = angularVelocity; } + + /** + * set both the linear and angular velocity of the object (using 6 doubles) + * @param[in] dx the x-axis linear velocity of the object + * @param[in] dy the y-axis linear velocity of the object + * @param[in] dz the z-axis linear velocity of the object + * @param[in] wx the x-axis angular velocity of the object + * @param[in] wy the y-axis angular velocity of the object + * @param[in] wz the z-axis angular velocity of the object + */ virtual void setVelocity(double dx, double dy, double dz, double wx, double wy, double wz) { linVel_ = {dx, dy, dz}; angVel_ = {wx, wy, wz}; } - void setLinearVelocity(const Vec<3>& linVel) { linVel_ = linVel; } + /** + * set only the linear velocity + * @param[in] linearVelocity the linear velocity + */ + void setLinearVelocity(const Vec<3>& linearVelocity) { linVel_ = linearVelocity; } - void setAngularVelocity(const Vec<3>& angVel) { angVel_ = angVel; } + /** + * set only the angular velocity + * @param[in] angVel the angular velocity + */ + void setAngularVelocity(const Vec<3>& angularVelocity) { angVel_ = angularVelocity; } + /** + * set external force on the object + * @param[in] localIdx this should always be 0 (because the single body object only has one body) + * @param[in] force force acting on the center of the mass (expressed in the world frame) + */ void setExternalForce(size_t localIdx, const Vec<3>& force) final; + + /** + * set external torque on the object + * @param[in] localIdx this should always be 0 (because the single body object only has one body) + * @param[in] torque torque acting on body (expressed in the world frame) + */ void setExternalTorque(size_t localIdx, const Vec<3>& torque) final; + + /** + * set external force on the object + * @param[in] localIdx this should always be 0 (because the single body object only has one body) + * @param[in] pos the application point of the force (expressed in the world frame) + * @param[in] force force acting on the center of the mass (expressed in the world frame) + */ void setExternalForce(size_t localIdx, const Vec<3>& pos, const Vec<3>& force) final; + + /// should not be used by the users void setConstraintForce(size_t localIdx, const Vec<3>& pos, const Vec<3>& force) final; void setGyroscopicMode(GyroscopicMode gyroscopicMode); + + /** + * Get position of a point on the body + * @param[in] localIdx this should always be 0 (because the single body object only has one body) + * @param[in] pos_b the position of the body + * @param[out] pos_w the corresponding position in the world + */ void getPosition(size_t localIdx, const Vec<3>& pos_b, Vec<3>& pos_w) const final; + + /** + * Get the geometric center of the object + * @param[out] pos_w the geometric center + */ void getPosition(Vec<3>& pos_w) { pos_w = bodyPosition_; }; void getVelocity(size_t localIdx, Vec<3>& vel_w) const final { vel_w = linVel_; } @@ -268,18 +418,60 @@ class SingleBodyObject : public Object { void integrate(double dt, const Vec<3>& gravity) final; void getContactPointVel(size_t pointId, Vec<3> &vel) const final; - virtual void updateCollision() override; + void updateCollision() override; + /** + * Set the linear damping that the object experiences due to air + * @param[in] damping the damping coefficient + */ void setLinearDamping(double damping); + + /** + * Set the angular damping that the object experiences due to air (proportional to the angular velocity) + * @param[in] damping the damping coefficient + */ void setAngularDamping(Vec<3> damping); + + /** + * set the body type. Dynamic means that object is free to move. + * Kinematic means that the object can have velocity but has an infinite mass (like a conveyor belt). + * Static means that the object cannot move and has an infinite mass. + * @param[in] type the body type + */ virtual void setBodyType(BodyType type); + /** + * get the current collision group of the object. Read the "Contact and Collision" to learn what the collision group is. + * @return the collision group + */ CollisionGroup getCollisionGroup(); + + /** + * get the current collision mast of the object. Read the "Contact and Collision" to learn what the collision mast is. + * @return the collision mast + */ CollisionGroup getCollisionMask(); + + /** + * set the appearance of the object. This works in both RaisimUnity and RaisimUnreal. + * But depending on the visualizer, they might do different things. + * You can specify the color by name like "blue", "green", "red" + * You can also specify the color by a string like "0, 0.5, 0.5, 1.0", which represent the RGBA values + * @param[in] appearance the appearance of the object + */ void setAppearance(const std::string& appearance) { appearance_ = appearance; } - const std::string& getAppearance() const { return appearance_; } + /** + * get the current appearance of the object + * @return appearance + */ + [[nodiscard]] const std::string& getAppearance() const { return appearance_; } + + /** + * delete all external forces and torques specified on the object. + * This method is called at the end of every frame. + */ void clearExternalForcesAndTorques() override { isExternalForces_.resize(0); externalForceAndTorque_.resize(0); diff --git a/raisim/m1/include/raisim/server/Charts.hpp b/raisim/m1/include/raisim/server/Charts.hpp index f7846114..b84cab47 100644 --- a/raisim/m1/include/raisim/server/Charts.hpp +++ b/raisim/m1/include/raisim/server/Charts.hpp @@ -13,6 +13,8 @@ namespace raisim { class Chart { + friend class raisim::RaisimServer; + public: virtual char* initialize(char* data) = 0; virtual char* serialize(char* data) = 0; @@ -24,18 +26,27 @@ class Chart { BAR_CHART } type_; + uint32_t visualTag = 0; public: Type getType() { return type_; } }; class TimeSeriesGraph : public Chart { - public: + friend class raisim::RaisimServer; + + protected: + // You should create time series graph using RaisimServer TimeSeriesGraph(std::string title, std::vector names, std::string xAxis, std::string yAxis) : size_(names.size()), xAxis_(std::move(xAxis)), yAxis_(std::move(yAxis)), names_(std::move(names)) { title_ = std::move(title); type_ = Type::TIME_SERIES; } + public: + /** + * Please read the "atlas" example to see how it works. + * @param[in] time x coordinate for the following data + * @param[in] d y coordinates of the data. The order is given when the chart is created */ void addDataPoints(double time, const raisim::VecDyn& d) { RSFATAL_IF(size_ != d.n, "Dimension mismatch. The chart has " << size_ << " categories and the inserted data is " << d.n << "dimension"); { @@ -51,6 +62,7 @@ class TimeSeriesGraph : public Chart { } } +protected: // not for users // void clearData() { timeStamp_ = std::queue(); @@ -92,13 +104,20 @@ class TimeSeriesGraph : public Chart { }; class BarChart : public Chart { - public: + friend class raisim::RaisimServer; + + protected: BarChart(std::string title, std::vector names) : size_(names.size()), names_(std::move(names)) { title_ = std::move(title); type_ = Type::BAR_CHART; } + public: + + /** + * Please read the "atlas" example to see how it works. + * @param[in] data values of each category. The histogram will be normalized. */ void setData(const std::vector& data) { RSFATAL_IF(size_ != data.size(), "Dimension mismatch. The chart has " << size_ << " categories and the inserted data is " << data.size() << "dimension"); { @@ -107,6 +126,7 @@ class BarChart : public Chart { } } + protected: char* initialize(char* data) final { using namespace server; return set(data, title_, names_); diff --git a/raisim/m1/include/raisim/server/Visuals.hpp b/raisim/m1/include/raisim/server/Visuals.hpp index 20531272..73cc1ece 100644 --- a/raisim/m1/include/raisim/server/Visuals.hpp +++ b/raisim/m1/include/raisim/server/Visuals.hpp @@ -15,11 +15,12 @@ namespace raisim { class RaisimServer; struct PolyLine { + friend class raisim::RaisimServer; + std::string name; Vec<4> color = {1, 1, 1, 1}; std::vector> points; double width = 0.01; - /** * @param[in] r red value of the color (max=1). * @param[in] g green value of the color (max=1). @@ -36,9 +37,14 @@ struct PolyLine { /** * clear all polyline points. */ void clearPoints() { points.clear(); } + + protected: + uint32_t visualTag = 0; }; struct ArticulatedSystemVisual { + friend class raisim::RaisimServer; + ArticulatedSystemVisual(const std::string &urdfFile) : obj(urdfFile) { color.setZero(); } @@ -64,19 +70,15 @@ struct ArticulatedSystemVisual { raisim::Vec<4> color; ArticulatedSystem obj; + protected: + uint32_t visualTag = 0; + std::string name; }; struct Visuals { - enum VisualType : int { - VisualSphere = 0, - VisualBox, - VisualCylinder, - VisualCapsule, - VisualMesh, - VisualArrow - }; + friend class raisim::RaisimServer; - VisualType type; + Shape::Type type; std::string name; std::string material; std::string meshFileName; @@ -93,7 +95,7 @@ struct Visuals { * capsule {radius, length, 0} * mesh {xscale, yscale, zscale} */ - Vec<3> size = {0, 0, 0}; + Vec<4> size = {0, 0, 0, 0}; /** * @param[in] radius the raidus of the sphere. @@ -165,19 +167,15 @@ struct Visuals { private: Vec<3> position = {0, 0, 0}; Vec<4> quaternion = {1, 0, 0, 0}; + + protected: + uint32_t visualTag = 0; }; struct InstancedVisuals { friend class raisim::RaisimServer; - enum VisualType : int { - VisualSphere = 0, - VisualBox, - VisualCylinder, - VisualArrow - }; - - InstancedVisuals(VisualType type, + InstancedVisuals(Shape::Type type, std::string name, const Vec<3>& size, const Vec<4>& color1, @@ -205,6 +203,7 @@ struct InstancedVisuals { for (auto& d: data) { d.quat = Vec<4>{1, 0, 0, 0}; d.scale = Vec<3>{1, 1, 1}; + d.scale = size; } } @@ -212,24 +211,26 @@ struct InstancedVisuals { * @param[in] pos position of the visual object in Eigen::Vector3d. * @param[in] ori quaternion of the visual object in Eigen::Vector4d. * @param[in] scale scale of the visual object in Eigen::Vector3d. + * @param[in] colorWeight the final color is an weighted average of color1 and color2 * add a new instance of the specified pose */ void addInstance(const Eigen::Vector3d &pos, const Eigen::Vector4d &ori, const Eigen::Vector3d& scale, float colorWeight = 0.f) { data.emplace_back(); data.back().pos = pos; data.back().quat = ori; - data.back().scale = scale; + data.back().scale = {size[0]*scale[0], size[1]*scale[1], size[2]*scale[2]}; data.back().colorWeight = colorWeight; } /** * @param[in] pos position of the visual object in Eigen::Vector3d. * @param[in] ori quaternion of the visual object in Eigen::Vector4d. + * @param[in] colorWeight the final color is an weighted average of color1 and color2 * add a new instance of the specified pose */ void addInstance(const Eigen::Vector3d &pos, const Eigen::Vector4d &ori, float colorWeight = 0.f) { data.emplace_back(); data.back().pos = pos; data.back().quat = ori; - data.back().scale = raisim::Vec<3>{1,1,1}; + data.back().scale = size; data.back().colorWeight = colorWeight; } @@ -240,7 +241,7 @@ struct InstancedVisuals { data.emplace_back(); data.back().pos = pos; data.back().quat = raisim::Vec<4>{1,0,0,0}; - data.back().scale = raisim::Vec<3>{1,1,1}; + data.back().scale = size; data.back().colorWeight = colorWeight; } @@ -304,7 +305,7 @@ struct InstancedVisuals { }; Vec<3> size; std::vector data; - VisualType type; + Shape::Type type; std::string name; std::string material; std::string meshFileName; @@ -312,6 +313,8 @@ struct InstancedVisuals { // {r, g, b, a} Vec<4> color1 = {1, 1, 1, 1}; Vec<4> color2 = {1, 1, 1, 1}; + + uint32_t visualTag = 0; }; } diff --git a/raisim/m1/lib/cmake/raisim/raisim-targets-release.cmake b/raisim/m1/lib/cmake/raisim/raisim-targets-release.cmake index 7589f154..904f081f 100644 --- a/raisim/m1/lib/cmake/raisim/raisim-targets-release.cmake +++ b/raisim/m1/lib/cmake/raisim/raisim-targets-release.cmake @@ -38,22 +38,22 @@ list(APPEND _IMPORT_CHECK_FILES_FOR_raisim::raisimMine "${_IMPORT_PREFIX}/lib/li # Import target "raisim::raisimODE" for configuration "RELEASE" set_property(TARGET raisim::raisimODE APPEND PROPERTY IMPORTED_CONFIGURATIONS RELEASE) set_target_properties(raisim::raisimODE PROPERTIES - IMPORTED_LOCATION_RELEASE "${_IMPORT_PREFIX}/lib/libraisimODE.1.1.3.dylib" - IMPORTED_SONAME_RELEASE "@rpath/libraisimODE.1.1.3.dylib" + IMPORTED_LOCATION_RELEASE "${_IMPORT_PREFIX}/lib/libraisimODE.1.1.4.dylib" + IMPORTED_SONAME_RELEASE "@rpath/libraisimODE.1.1.4.dylib" ) list(APPEND _IMPORT_CHECK_TARGETS raisim::raisimODE ) -list(APPEND _IMPORT_CHECK_FILES_FOR_raisim::raisimODE "${_IMPORT_PREFIX}/lib/libraisimODE.1.1.3.dylib" ) +list(APPEND _IMPORT_CHECK_FILES_FOR_raisim::raisimODE "${_IMPORT_PREFIX}/lib/libraisimODE.1.1.4.dylib" ) # Import target "raisim::raisim" for configuration "RELEASE" set_property(TARGET raisim::raisim APPEND PROPERTY IMPORTED_CONFIGURATIONS RELEASE) set_target_properties(raisim::raisim PROPERTIES - IMPORTED_LOCATION_RELEASE "${_IMPORT_PREFIX}/lib/libraisim.1.1.3.dylib" - IMPORTED_SONAME_RELEASE "@rpath/libraisim.1.1.3.dylib" + IMPORTED_LOCATION_RELEASE "${_IMPORT_PREFIX}/lib/libraisim.1.1.4.dylib" + IMPORTED_SONAME_RELEASE "@rpath/libraisim.1.1.4.dylib" ) list(APPEND _IMPORT_CHECK_TARGETS raisim::raisim ) -list(APPEND _IMPORT_CHECK_FILES_FOR_raisim::raisim "${_IMPORT_PREFIX}/lib/libraisim.1.1.3.dylib" ) +list(APPEND _IMPORT_CHECK_FILES_FOR_raisim::raisim "${_IMPORT_PREFIX}/lib/libraisim.1.1.4.dylib" ) # Commands beyond this point should not need to know the version. set(CMAKE_IMPORT_FILE_VERSION) diff --git a/raisim/m1/lib/cmake/raisim/raisimConfig-version.cmake b/raisim/m1/lib/cmake/raisim/raisimConfig-version.cmake index 23eb1d7d..91779a9c 100644 --- a/raisim/m1/lib/cmake/raisim/raisimConfig-version.cmake +++ b/raisim/m1/lib/cmake/raisim/raisimConfig-version.cmake @@ -7,7 +7,7 @@ # PACKAGE_VERSION_COMPATIBLE if the current version is >= requested version. # The variable CVF_VERSION must be set before calling configure_file(). -set(PACKAGE_VERSION "1.1.3") +set(PACKAGE_VERSION "1.1.4") if (PACKAGE_FIND_VERSION_RANGE) # Package version must be in the requested version range diff --git a/raisim/m1/lib/libraisim.1.1.4.dylib b/raisim/m1/lib/libraisim.1.1.4.dylib new file mode 100755 index 00000000..48e5d613 Binary files /dev/null and b/raisim/m1/lib/libraisim.1.1.4.dylib differ diff --git a/raisim/m1/lib/libraisim.dylib b/raisim/m1/lib/libraisim.dylib index b227ab09..482a7748 120000 --- a/raisim/m1/lib/libraisim.dylib +++ b/raisim/m1/lib/libraisim.dylib @@ -1 +1 @@ -libraisim.1.1.3.dylib \ No newline at end of file +libraisim.1.1.4.dylib \ No newline at end of file diff --git a/raisim/m1/lib/libraisimODE.1.1.4.dylib b/raisim/m1/lib/libraisimODE.1.1.4.dylib new file mode 100755 index 00000000..2b564d65 Binary files /dev/null and b/raisim/m1/lib/libraisimODE.1.1.4.dylib differ diff --git a/raisim/m1/lib/libraisimODE.dylib b/raisim/m1/lib/libraisimODE.dylib index d1a59635..56c3b452 120000 --- a/raisim/m1/lib/libraisimODE.dylib +++ b/raisim/m1/lib/libraisimODE.dylib @@ -1 +1 @@ -libraisimODE.1.1.3.dylib \ No newline at end of file +libraisimODE.1.1.4.dylib \ No newline at end of file diff --git a/raisimUnity/m1/RaiSimUnity.app/Contents/Info.plist b/raisimUnity/m1/RaiSimUnity.app/Contents/Info.plist index 6e084177..c7fb8077 100644 --- a/raisimUnity/m1/RaiSimUnity.app/Contents/Info.plist +++ b/raisimUnity/m1/RaiSimUnity.app/Contents/Info.plist @@ -11,7 +11,7 @@ CFBundleIconFile PlayerIcon.icns CFBundleIdentifier - com.UnityTechnologies.BotD.Environment + com.RaiSimTech.RaiSimUnity CFBundleInfoDictionaryVersion 6.0 CFBundleName diff --git a/raisimUnity/m1/RaiSimUnity.app/Contents/MacOS/RaiSimUnity b/raisimUnity/m1/RaiSimUnity.app/Contents/MacOS/RaiSimUnity index 0d8d9e71..66f6e410 100755 Binary files a/raisimUnity/m1/RaiSimUnity.app/Contents/MacOS/RaiSimUnity and b/raisimUnity/m1/RaiSimUnity.app/Contents/MacOS/RaiSimUnity differ diff --git a/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/Managed/Assembly-CSharp.dll b/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/Managed/Assembly-CSharp.dll index 3c730e68..4f8ff674 100644 Binary files a/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/Managed/Assembly-CSharp.dll and b/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/Managed/Assembly-CSharp.dll differ diff --git a/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/globalgamemanagers b/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/globalgamemanagers index d6ca6a68..20f74da4 100644 Binary files a/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/globalgamemanagers and b/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/globalgamemanagers differ diff --git a/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/globalgamemanagers.assets b/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/globalgamemanagers.assets index 0b207885..5fbaaaf2 100644 Binary files a/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/globalgamemanagers.assets and b/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/globalgamemanagers.assets differ diff --git a/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/level0 b/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/level0 index 547dfe02..ee0ed23e 100644 Binary files a/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/level0 and b/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/level0 differ diff --git a/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/resources.assets b/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/resources.assets index 52941fe8..63c56117 100644 Binary files a/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/resources.assets and b/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/resources.assets differ diff --git a/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/sharedassets0.assets b/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/sharedassets0.assets index 87c8eaba..8200f791 100644 Binary files a/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/sharedassets0.assets and b/raisimUnity/m1/RaiSimUnity.app/Contents/Resources/Data/sharedassets0.assets differ diff --git a/raisimUnity/m1/RaiSimUnity.app/Contents/_CodeSignature/CodeResources b/raisimUnity/m1/RaiSimUnity.app/Contents/_CodeSignature/CodeResources index 37cba972..2c13e3c1 100644 --- a/raisimUnity/m1/RaiSimUnity.app/Contents/_CodeSignature/CodeResources +++ b/raisimUnity/m1/RaiSimUnity.app/Contents/_CodeSignature/CodeResources @@ -10,7 +10,7 @@ Resources/Data/Managed/Assembly-CSharp.dll - /X9KaVbep4c5/bd2c5eIVHcRbUE= + 2+qnjKC8YwS1EWY5IPC7wcxNECg= Resources/Data/Managed/AssimpNet.dll @@ -502,11 +502,11 @@ Resources/Data/globalgamemanagers - UTwQqobTVfwXLRFnl3GhVfZSUew= + vyHN5OTbaozJeqUtrivfZymHc1o= Resources/Data/globalgamemanagers.assets - fggJO/FtuVfGX5vjrJrGIi8fdcw= + 9CS5r+p4tMj9f/0tO75Ky5/UyeM= Resources/Data/globalgamemanagers.assets.resS @@ -514,7 +514,7 @@ Resources/Data/level0 - ANy3FxKRnvUCG80Eq8ah1giMS8o= + H6giqN+vb2Bmbi+xkwULRYTqln0= Resources/Data/level0.resS @@ -522,7 +522,7 @@ Resources/Data/resources.assets - ZRWM/Vcg3zo+/P2ZwdxocBgt67A= + z1Mee/LA/9aOEItJpurEw8RGIeU= Resources/Data/resources.assets.resS @@ -530,7 +530,7 @@ Resources/Data/sharedassets0.assets - QPR58bV2BD0nYGO4EeHd/HxUBDc= + XhmDKAQDTB8nA4rtkmdyJci8Bk4= Resources/Data/sharedassets0.assets.resS @@ -728,7 +728,7 @@ hash2 - Okt2iFniIat2IHaaYYUNk4LdzPW2bX884naD1o2Tx7M= + zUNM95ggG2StqVdLIdT4VlxSKjCveVPFu7oE+Q6Lqho= Resources/Data/Managed/AssimpNet.dll @@ -1589,14 +1589,14 @@ hash2 - mauO84491nVkdfN2mjKux9Fkj2yD3+ZL9E8q9s1MPxc= + +K4TAJdlj/d2bg6XirhqeyhDj9kI4Cbqt0AIp8nJUJY= Resources/Data/globalgamemanagers.assets hash2 - axj+8PXIaiU0nd83jAn+wlD7ZP354sb8YmCzqkrdVao= + 1/oNoBSz/1qgIWy8zuQdZKpvQ7/2gZUN6IQuqLNjRP8= Resources/Data/globalgamemanagers.assets.resS @@ -1610,7 +1610,7 @@ hash2 - r/jCthKBIVBz9ESy2BsemE0HuFzIoIiqemW+AP/wtvE= + hhxriYVC7+5Lrjtd+c3eaKNENZxm+KTOwZ0DYZHL/O0= Resources/Data/level0.resS @@ -1624,7 +1624,7 @@ hash2 - ziO+pitiPf3Rrhc8+BcG+J5sB8lQId1WZgh4/wlhVhE= + 1C7mEOC2vZue1lsY+uxknfmBsUv+bfRv8BhuQUXP6WU= Resources/Data/resources.assets.resS @@ -1638,7 +1638,7 @@ hash2 - nNBu+m8TogGqRKsi6U9eN8dlPa0BRPb8gHcxv6Rd8BE= + uV8UGwjUhT99uLCz6MJrCosDr8925OTmq/Y7N+SpmlI= Resources/Data/sharedassets0.assets.resS diff --git a/raisimUnity/m1/RaiSimUnity.app/Logs/raisim_error_log.txt b/raisimUnity/m1/RaiSimUnity.app/Logs/raisim_error_log.txt index 4ffe77ae..d8ad9fa1 100644 --- a/raisimUnity/m1/RaiSimUnity.app/Logs/raisim_error_log.txt +++ b/raisimUnity/m1/RaiSimUnity.app/Logs/raisim_error_log.txt @@ -1,11 +1,25 @@ error logs -System.Exception: RsUnityRemote: UpdateObjectPosition -Unable to read data from the transport connection: Connection reset by peer. - at System.Net.Sockets.NetworkStream.Read (System.Byte[] buffer, System.Int32 offset, System.Int32 size) [0x000e2] in <6f1d457dbe7848f19617dea9acc2268a>:0 - at raisimUnity.TcpHelper.ReadData () [0x0002b] in <2265bd80598a49d7ae7e24a6682a0295>:0 - at raisimUnity.RsUnityRemote.ReadAndCheckServer (raisimUnity.ClientMessageType type) [0x000b4] in <2265bd80598a49d7ae7e24a6682a0295>:0 - at raisimUnity.RsUnityRemote.Update () [0x00371] in <2265bd80598a49d7ae7e24a6682a0295>:0 - at raisimUnity.RsuException..ctor (System.Exception ex, System.String message) [0x00037] in <2265bd80598a49d7ae7e24a6682a0295>:0 - at raisimUnity.RsUnityRemote.Update () [0x003ba] in <2265bd80598a49d7ae7e24a6682a0295>:0 +System.AggregateException: One or more errors occurred. ---> System.IO.IOException: Unable to read data from the transport connection: Connection reset by peer. ---> System.Net.Sockets.SocketException: Connection reset by peer + at System.Net.Sockets.Socket.EndReceive (System.IAsyncResult asyncResult) [0x00012] in <6f1d457dbe7848f19617dea9acc2268a>:0 + at System.Net.Sockets.NetworkStream.EndRead (System.IAsyncResult asyncResult) [0x00057] in <6f1d457dbe7848f19617dea9acc2268a>:0 + --- End of inner exception stack trace --- + at System.Net.Sockets.NetworkStream.EndRead (System.IAsyncResult asyncResult) [0x0009b] in <6f1d457dbe7848f19617dea9acc2268a>:0 + at System.IO.Stream+<>c.b__43_1 (System.IO.Stream stream, System.IAsyncResult asyncResult) [0x00000] in :0 + at System.Threading.Tasks.TaskFactory`1+FromAsyncTrimPromise`1[TResult,TInstance].Complete (TInstance thisRef, System.Func`3[T1,T2,TResult] endMethod, System.IAsyncResult asyncResult, System.Boolean requiresSynchronization) [0x00000] in :0 + --- End of inner exception stack trace --- + at System.Threading.Tasks.Task.ThrowIfExceptional (System.Boolean includeTaskCanceledExceptions) [0x00011] in :0 + at System.Threading.Tasks.Task`1[TResult].GetResultCore (System.Boolean waitCompletionNotification) [0x0002b] in :0 + at System.Threading.Tasks.Task`1[TResult].get_Result () [0x0000f] in :0 + at raisimUnity.TcpHelper.ReadData () [0x00044] in :0 + at raisimUnity.RsUnityRemote.ReadAndCheckServer (raisimUnity.ClientMessageType type) [0x00094] in :0 + at raisimUnity.RsUnityRemote.Update () [0x0002a] in :0 +---> (Inner Exception #0) System.IO.IOException: Unable to read data from the transport connection: Connection reset by peer. ---> System.Net.Sockets.SocketException: Connection reset by peer + at System.Net.Sockets.Socket.EndReceive (System.IAsyncResult asyncResult) [0x00012] in <6f1d457dbe7848f19617dea9acc2268a>:0 + at System.Net.Sockets.NetworkStream.EndRead (System.IAsyncResult asyncResult) [0x00057] in <6f1d457dbe7848f19617dea9acc2268a>:0 + --- End of inner exception stack trace --- + at System.Net.Sockets.NetworkStream.EndRead (System.IAsyncResult asyncResult) [0x0009b] in <6f1d457dbe7848f19617dea9acc2268a>:0 + at System.IO.Stream+<>c.b__43_1 (System.IO.Stream stream, System.IAsyncResult asyncResult) [0x00000] in :0 + at System.Threading.Tasks.TaskFactory`1+FromAsyncTrimPromise`1[TResult,TInstance].Complete (TInstance thisRef, System.Func`3[T1,T2,TResult] endMethod, System.IAsyncResult asyncResult, System.Boolean requiresSynchronization) [0x00000] in :0 <--- +