Merge pull request #510 from jhwangbo/master

fix sensor update time, getGeneralizedVelocityIndex

Author: jhwangbo

raisim/linux/include/raisim/RaisimServer.hpp

@@ -1186,6 +1186,7 @@ class RaisimServer final {
               < world_->getWorldTime() + 1e-10) {
         data_ = set(data_, true);
         needsSensorUpdate_ = true;
+        sensorUpdateTime_ = world_->getWorldTime();
       } else {
         data_ = set(data_, false);
       }
@@ -1691,14 +1692,14 @@ class RaisimServer final {
         auto &img = std::static_pointer_cast<RGBCamera>(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);
-        sensor->setUpdateTimeStamp(world_->getWorldTime());
+        sensor->setUpdateTimeStamp(sensorUpdateTime_);
       } else if (type == Sensor::Type::DEPTH) {
         int width, height;
         rData_ = get(rData_, &width, &height);
         auto &depthArray = std::static_pointer_cast<DepthCamera>(sensor)->getDepthArray();
         RSFATAL_IF(width * height != depthArray.size(), "Image size mismatch. Sensor module not working properly")
         rData_ = getN(rData_, depthArray.data(), width * height);
-        sensor->setUpdateTimeStamp(world_->getWorldTime());
+        sensor->setUpdateTimeStamp(sensorUpdateTime_);
       }
     }
 
@@ -1707,6 +1708,7 @@ class RaisimServer final {
 
   char *data_, *rData_;
   bool needsSensorUpdate_ = false;
+  double sensorUpdateTime_;
   World *world_;
   std::vector<char> receive_buffer, send_buffer;
   bool connected_ = false;

raisim/linux/include/raisim/object/ArticulatedSystem/ArticulatedSystem.hpp

@@ -15,6 +15,7 @@
 #include <cmath>
 #include <unordered_map>
 #include <fstream>
+#include <map>
 #include "raisim/math.hpp"
 #include "algorithm"
 
@@ -560,6 +561,14 @@ class ArticulatedSystem : public Object {
    * @return movable joint names in the joint order. */
   [[nodiscard]] const std::vector<std::string> &getMovableJointNames() const { return movableJointNames; };
 
+  /**
+   * get generalized velocity index of a joint.
+   * @return the joint index. */
+  [[nodiscard]] size_t getGeneralizedVelocityIndex(const std::string& name) const {
+    RSFATAL_IF(jointName2GvIdx.find(name) == jointName2GvIdx.end(), name + " not found")
+    return jointName2GvIdx.at(name);
+  };
+
   /**
    * @param[in] bodyIdx The body which contains the point, can be retrieved by getBodyIdx()
    * @param[in] point_B The position of the point in the body frame
@@ -1573,23 +1582,23 @@ class ArticulatedSystem : public Object {
    *
    * @return return if the inverse dynamics is computed
    */
-  bool getComputeInverseDynamics() const { return computeInverseDynamics_; }
+  [[nodiscard]] bool getComputeInverseDynamics() const { return computeInverseDynamics_; }
 
   /**
    * YOU MUST CALL raisim::ArticulatedSystem::setComputeInverseDynamics(true) before calling this method.
    * This method returns the force at the specified joint
    * @param jointId[in] the joint id
    * @return force at the joint
    */
-  const raisim::Vec<3>& getForceAtJointInWorldFrame(size_t jointId) const { return forceAtJoint_W[jointId]; }
+  [[nodiscard]] const raisim::Vec<3>& getForceAtJointInWorldFrame(size_t jointId) const { return forceAtJoint_W[jointId]; }
 
   /**
    * YOU MUST CALL raisim::ArticulatedSystem::setComputeInverseDynamics(true) before calling this method.
    * This method returns the torque at the specified joint
    * @param jointId[in] the joint id
    * @return torque at the joint
    */
-  const raisim::Vec<3>& getTorqueAtJointInWorldFrame(size_t jointId) const { return torqueAtJoint_W[jointId]; }
+  [[nodiscard]] const raisim::Vec<3>& getTorqueAtJointInWorldFrame(size_t jointId) const { return torqueAtJoint_W[jointId]; }
 
  protected:
 
@@ -1746,6 +1755,7 @@ class ArticulatedSystem : public Object {
   std::vector<Child> rootChild_;
   std::vector<std::string> bodyName;
   std::vector<std::string> movableJointNames;
+  std::map<std::string, size_t> jointName2GvIdx;
 
   std::vector<SparseJacobian> contactJaco_;
   std::vector<SparseJacobian> externalForceAndTorqueJaco_;

raisim/linux/lib/libraisim.so.1.1.7

@@ -1186,6 +1186,7 @@ class RaisimServer final {
               < world_->getWorldTime() + 1e-10) {
         data_ = set(data_, true);
         needsSensorUpdate_ = true;
+        sensorUpdateTime_ = world_->getWorldTime();
       } else {
         data_ = set(data_, false);
       }
@@ -1691,14 +1692,14 @@ class RaisimServer final {
         auto &img = std::static_pointer_cast<RGBCamera>(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);
-        sensor->setUpdateTimeStamp(world_->getWorldTime());
+        sensor->setUpdateTimeStamp(sensorUpdateTime_);
       } else if (type == Sensor::Type::DEPTH) {
         int width, height;
         rData_ = get(rData_, &width, &height);
         auto &depthArray = std::static_pointer_cast<DepthCamera>(sensor)->getDepthArray();
         RSFATAL_IF(width * height != depthArray.size(), "Image size mismatch. Sensor module not working properly")
         rData_ = getN(rData_, depthArray.data(), width * height);
-        sensor->setUpdateTimeStamp(world_->getWorldTime());
+        sensor->setUpdateTimeStamp(sensorUpdateTime_);
       }
     }
 
@@ -1707,6 +1708,7 @@ class RaisimServer final {
 
   char *data_, *rData_;
   bool needsSensorUpdate_ = false;
+  double sensorUpdateTime_;
   World *world_;
   std::vector<char> receive_buffer, send_buffer;
   bool connected_ = false;

raisim/linux/lib/raisimpy.cpython-310-x86_64-linux-gnu.so

@@ -15,6 +15,7 @@
 #include <cmath>
 #include <unordered_map>
 #include <fstream>
+#include <map>
 #include "raisim/math.hpp"
 #include "algorithm"
 
@@ -560,6 +561,14 @@ class ArticulatedSystem : public Object {
    * @return movable joint names in the joint order. */
   [[nodiscard]] const std::vector<std::string> &getMovableJointNames() const { return movableJointNames; };
 
+  /**
+   * get generalized velocity index of a joint.
+   * @return the joint index. */
+  [[nodiscard]] size_t getGeneralizedVelocityIndex(const std::string& name) const {
+    RSFATAL_IF(jointName2GvIdx.find(name) == jointName2GvIdx.end(), name + " not found")
+    return jointName2GvIdx.at(name);
+  };
+
   /**
    * @param[in] bodyIdx The body which contains the point, can be retrieved by getBodyIdx()
    * @param[in] point_B The position of the point in the body frame
@@ -1504,6 +1513,13 @@ class ArticulatedSystem : public Object {
     return jointLimits_;
   }
 
+  /**
+   * get the joint velocity limits
+   * @return jointLimits joint velocity limits*/
+    const VecDyn& getJointVelocityLimits() {
+      return velLimits_;
+    }
+
   /**
    * Clears all external forces and torques */
   void clearExternalForcesAndTorques() final {
@@ -1566,23 +1582,23 @@ class ArticulatedSystem : public Object {
    *
    * @return return if the inverse dynamics is computed
    */
-  bool getComputeInverseDynamics() const { return computeInverseDynamics_; }
+  [[nodiscard]] bool getComputeInverseDynamics() const { return computeInverseDynamics_; }
 
   /**
    * YOU MUST CALL raisim::ArticulatedSystem::setComputeInverseDynamics(true) before calling this method.
    * This method returns the force at the specified joint
    * @param jointId[in] the joint id
    * @return force at the joint
    */
-  const raisim::Vec<3>& getForceAtJointInWorldFrame(size_t jointId) const { return forceAtJoint_W[jointId]; }
+  [[nodiscard]] const raisim::Vec<3>& getForceAtJointInWorldFrame(size_t jointId) const { return forceAtJoint_W[jointId]; }
 
   /**
    * YOU MUST CALL raisim::ArticulatedSystem::setComputeInverseDynamics(true) before calling this method.
    * This method returns the torque at the specified joint
    * @param jointId[in] the joint id
    * @return torque at the joint
    */
-  const raisim::Vec<3>& getTorqueAtJointInWorldFrame(size_t jointId) const { return torqueAtJoint_W[jointId]; }
+  [[nodiscard]] const raisim::Vec<3>& getTorqueAtJointInWorldFrame(size_t jointId) const { return torqueAtJoint_W[jointId]; }
 
  protected:
 
@@ -1739,6 +1755,7 @@ class ArticulatedSystem : public Object {
   std::vector<Child> rootChild_;
   std::vector<std::string> bodyName;
   std::vector<std::string> movableJointNames;
+  std::map<std::string, size_t> jointName2GvIdx;
 
   std::vector<SparseJacobian> contactJaco_;
   std::vector<SparseJacobian> externalForceAndTorqueJaco_;