spring works

raisim/linux/include/raisim/math.hpp

@@ -495,7 +495,6 @@ class SparseJacobian {
 
   inline void resize(size_t cols) {
     size = cols;
-    DRSFATAL_IF(cols<1, "assigning zero volume")
     if(capacity < cols){
       v.resize(3, cols);
       v.setZero();
@@ -1446,6 +1445,12 @@ inline void crossThenAdd(const Vec<3> &vec1, const Vec<3> &vec2, Vec<3> &vec) {
   vec[2] += vec1[0] * vec2[1] - vec1[1] * vec2[0];
 }
 
+inline void crossThenAdd(const Vec<3> &vec1, const Vec<3> &vec2, double* vec) {
+  vec[0] += vec1[1] * vec2[2] - vec1[2] * vec2[1];
+  vec[1] += vec1[2] * vec2[0] - vec1[0] * vec2[2];
+  vec[2] += vec1[0] * vec2[1] - vec1[1] * vec2[0];
+}
+
 inline void crossThenAdd(const double *vec1, const double *vec2, double *vec) {
   vec[0] += vec1[1] * vec2[2] - vec1[2] * vec2[1];
   vec[1] += vec1[2] * vec2[0] - vec1[0] * vec2[2];

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

@@ -77,6 +77,9 @@ class ArticulatedSystem : public Object {
   struct SpringElement {
     SpringElement() { q_ref.setZero(); }
 
+    void setSpringMount (const Eigen::Vector4d& qRef) { q_ref = qRef; }
+    Eigen::Vector4d getSpringMount () { return q_ref.e(); }
+
     /// the spring connects this body and its parent
     size_t childBodyId = 0;
     /// spring stiffness
@@ -701,11 +704,30 @@ class ArticulatedSystem : public Object {
    * @param[out] jaco the positional Jacobian. v = J * u. v is the linear velocity expressed in the world frame and u is the generalized velocity */
   void getSparseJacobian(size_t bodyIdx, const Vec<3> &point_W, SparseJacobian &jaco) const;
 
+  /**
+   * @param[in] bodyIdx the body index. Note that body index and the joint index are the same because every body has one parent joint. It can be retrieved by getBodyIdx()
+   * @param[in] frame the frame in which the position of the point is expressed in
+   * @param[in] point the point expressed in the world frame. If you want to use a point expressed in the body frame, use getDenseFrameJacobian()
+   * @param[out] jaco the positional Jacobian. v = J * u. v is the linear velocity expressed in the world frame and u is the generalized velocity */
+  void getSparseJacobian(size_t bodyIdx, Frame frame, const Vec<3> &point, SparseJacobian &jaco) const;
+
   /**
    * @param[in] bodyIdx the body index. Note that body index and the joint index are the same because every body has one parent joint. It can be retrieved by getBodyIdx()
    * @param[out] jaco the rotational Jacobian. omega = J * u. omgea is the angular velocity expressed in the world frame and u is the generalized velocity */
   void getSparseRotationalJacobian(size_t bodyIdx, SparseJacobian &jaco) const;
 
+  /**
+   * @param[in] bodyIdx the body index. Note that body index and the joint index are the same because every body has one parent joint. It can be retrieved by getBodyIdx()
+   * @param[in] frame the frame in which the position of the point is expressed
+   * @param[in] point the position of the point of interest
+   * @param[out] jaco the time derivative of the positional Jacobian. a = dJ * u + J * du. a is the linear acceleration expressed in the world frame, u is the generalized velocity and d denotes the time derivative*/
+  void getTimeDerivativeOfSparseJacobian(size_t bodyIdx, Frame frame, const Vec<3> &point, SparseJacobian &jaco) const;
+
+  /**
+   * @param[in] bodyIdx the body index. Note that body index and the joint index are the same because every body has one parent joint. It can be retrieved by getBodyIdx()
+   * @param[out] jaco the rotational Jacobian. alpha = dJ * u + J * du. alpha is the angular acceleration expressed in the world frame, u is the generalized velocity and d denotes the time derivative*/
+  void getTimeDerivativeOfSparseRotationalJacobian(size_t bodyIdx, SparseJacobian &jaco) const;
+
   /**
    * @param[in] sparseJaco sparse Jacobian (either positional or rotational)
    * @param[out] denseJaco the corresponding dense Jacobian */
@@ -793,6 +815,14 @@ class ArticulatedSystem : public Object {
    * @param[out] pointVel the velocity of the point expressed in the world frame */
   void getVelocity(size_t bodyIdx, const Vec<3> &posInBodyFrame, Vec<3> &pointVel) const;
 
+  /**
+   * @param[in] bodyIdx the body index. it can be retrieved by getBodyIdx()
+   * @param[in] frameOfPos the frame in which the provided position is expressed
+   * @param[in] pos the position of the point of interest
+   * @param[in] frameOfVel the frame in which the computed velocity is expressed
+   * @param[out] pointVel the velocity of the point expressed in the world frame */
+  void getVelocity(size_t bodyIdx, Frame frameOfPos, const Vec<3> &pos, Frame frameOfVel, Vec<3> &pointVel) const;
+
   /**
    * returns the index of the body
    * @param[in] nm name of the body. The body name is the name of the movable link of the body
@@ -1413,15 +1443,20 @@ class ArticulatedSystem : public Object {
     externalForceAndTorqueJaco_.resize(0);
   }
 
-
   /**
    * @param[in] spring Additional spring elements for joints */
   void addSpring(const SpringElement &spring) { springs_.push_back(spring); }
 
   /**
    * @return springs Existing spring elements on joints */
   std::vector<SpringElement> &getSprings() { return springs_; }
-  const std::vector<SpringElement> &getSprings() const { return springs_; }
+//  const std::vector<SpringElement> &getSprings() const { return springs_; }
+
+  /**
+   *
+   * @return parent parent[i] is a parent body id of the i^th body
+   */
+  const std::vector<size_t>& getParentVector() const { return parent; }
 
   // not recommended for users. only for developers
   void addConstraints(const std::vector<PinConstraintDefinition>& pinDef);

raisim/linux/lib/cmake/raisim/raisim-targets-debug.cmake

@@ -0,0 +1,59 @@
+#----------------------------------------------------------------
+# Generated CMake target import file for configuration "DEBUG".
+#----------------------------------------------------------------
+
+# Commands may need to know the format version.
+set(CMAKE_IMPORT_FILE_VERSION 1)
+
+# Import target "raisim::raisimZ" for configuration "DEBUG"
+set_property(TARGET raisim::raisimZ APPEND PROPERTY IMPORTED_CONFIGURATIONS DEBUG)
+set_target_properties(raisim::raisimZ PROPERTIES
+  IMPORTED_LOCATION_DEBUG "${_IMPORT_PREFIX}/lib/libraisimZ.so"
+  IMPORTED_SONAME_DEBUG "libraisimZ.so"
+  )
+
+list(APPEND _IMPORT_CHECK_TARGETS raisim::raisimZ )
+list(APPEND _IMPORT_CHECK_FILES_FOR_raisim::raisimZ "${_IMPORT_PREFIX}/lib/libraisimZ.so" )
+
+# Import target "raisim::raisimPng" for configuration "DEBUG"
+set_property(TARGET raisim::raisimPng APPEND PROPERTY IMPORTED_CONFIGURATIONS DEBUG)
+set_target_properties(raisim::raisimPng PROPERTIES
+  IMPORTED_LOCATION_DEBUG "${_IMPORT_PREFIX}/lib/libraisimPng.so"
+  IMPORTED_SONAME_DEBUG "libraisimPng.so"
+  )
+
+list(APPEND _IMPORT_CHECK_TARGETS raisim::raisimPng )
+list(APPEND _IMPORT_CHECK_FILES_FOR_raisim::raisimPng "${_IMPORT_PREFIX}/lib/libraisimPng.so" )
+
+# Import target "raisim::raisimMine" for configuration "DEBUG"
+set_property(TARGET raisim::raisimMine APPEND PROPERTY IMPORTED_CONFIGURATIONS DEBUG)
+set_target_properties(raisim::raisimMine PROPERTIES
+  IMPORTED_LOCATION_DEBUG "${_IMPORT_PREFIX}/lib/libraisimMine.so"
+  IMPORTED_SONAME_DEBUG "libraisimMine.so"
+  )
+
+list(APPEND _IMPORT_CHECK_TARGETS raisim::raisimMine )
+list(APPEND _IMPORT_CHECK_FILES_FOR_raisim::raisimMine "${_IMPORT_PREFIX}/lib/libraisimMine.so" )
+
+# Import target "raisim::raisimODE" for configuration "DEBUG"
+set_property(TARGET raisim::raisimODE APPEND PROPERTY IMPORTED_CONFIGURATIONS DEBUG)
+set_target_properties(raisim::raisimODE PROPERTIES
+  IMPORTED_LOCATION_DEBUG "${_IMPORT_PREFIX}/lib/libraisimODE.so"
+  IMPORTED_SONAME_DEBUG "libraisimODE.so"
+  )
+
+list(APPEND _IMPORT_CHECK_TARGETS raisim::raisimODE )
+list(APPEND _IMPORT_CHECK_FILES_FOR_raisim::raisimODE "${_IMPORT_PREFIX}/lib/libraisimODE.so" )
+
+# Import target "raisim::raisim" for configuration "DEBUG"
+set_property(TARGET raisim::raisim APPEND PROPERTY IMPORTED_CONFIGURATIONS DEBUG)
+set_target_properties(raisim::raisim PROPERTIES
+  IMPORTED_LOCATION_DEBUG "${_IMPORT_PREFIX}/lib/libraisim.so"
+  IMPORTED_SONAME_DEBUG "libraisim.so"
+  )
+
+list(APPEND _IMPORT_CHECK_TARGETS raisim::raisim )
+list(APPEND _IMPORT_CHECK_FILES_FOR_raisim::raisim "${_IMPORT_PREFIX}/lib/libraisim.so" )
+
+# Commands beyond this point should not need to know the version.
+set(CMAKE_IMPORT_FILE_VERSION)

raisim/linux/lib/cmake/raisim/raisimConfig-version.cmake

@@ -18,14 +18,20 @@ else()
   endif()
 endif()
 
+
+# if the installed project requested no architecture check, don't perform the check
+if("FALSE")
+  return()
+endif()
+
 # if the installed or the using project don't have CMAKE_SIZEOF_VOID_P set, ignore it:
 if("${CMAKE_SIZEOF_VOID_P}" STREQUAL "" OR "8" STREQUAL "")
-   return()
+  return()
 endif()
 
 # check that the installed version has the same 32/64bit-ness as the one which is currently searching:
 if(NOT CMAKE_SIZEOF_VOID_P STREQUAL "8")
-   math(EXPR installedBits "8 * 8")
-   set(PACKAGE_VERSION "${PACKAGE_VERSION} (${installedBits}bit)")
-   set(PACKAGE_VERSION_UNSUITABLE TRUE)
+  math(EXPR installedBits "8 * 8")
+  set(PACKAGE_VERSION "${PACKAGE_VERSION} (${installedBits}bit)")
+  set(PACKAGE_VERSION_UNSUITABLE TRUE)
 endif()

raisimGymTorch/raisimGymTorch/env/RaisimGymEnv.hpp

@@ -15,12 +15,6 @@
 #include "Yaml.hpp"
 #include "Reward.hpp"
 
-#define __RSG_MAKE_STR(x) #x
-#define _RSG_MAKE_STR(x) __RSG_MAKE_STR(x)
-#define RSG_MAKE_STR(x) _RSG_MAKE_STR(x)
-
-#define READ_YAML(a, b, c) RSFATAL_IF(!&c, "Node "<<RSG_MAKE_STR(c)<<" doesn't exist") b = c.template As<a>();
-
 namespace raisim {
 
 using Dtype=float;

raisimPy/examples/robots.py

@@ -3,6 +3,7 @@
 import raisimpy as raisim
 import time
 
+
 raisim.World.setLicenseFile(os.path.dirname(os.path.abspath(__file__)) + "/../../rsc/activation.raisim")
 anymal_urdf_file = os.path.dirname(os.path.abspath(__file__)) + "/../../rsc/anymal/urdf/anymal.urdf"
 laikago_urdf_file = os.path.dirname(os.path.abspath(__file__)) + "/../../rsc/laikago/laikago.urdf"

raisimPy/examples/springs.py

@@ -0,0 +1,27 @@
+import os
+import numpy as np
+import raisimpy as raisim
+import time
+
+
+world = raisim.World()
+world.setTimeStep(0.001)
+
+springed_cartpole_urdf_file = os.path.dirname(os.path.abspath(__file__)) + "/../../rsc/springDamper/cartpole.urdf"
+server = raisim.RaisimServer(world)
+ground = world.addGround()
+
+springed_cartpole = world.addArticulatedSystem(springed_cartpole_urdf_file)
+springed_cartpole.setName("springed_cartpole")
+springed_cartpole.getSprings()[1].q_ref = np.array([2., 0, 0, 0]) # overwrite the spring mount position
+
+for spring in springed_cartpole.getSprings():
+    print(spring.q_ref)
+
+server.launchServer(8080)
+
+for i in range(500000):
+    time.sleep(0.001)
+    world.integrate()
+
+server.killServer()

raisimPy/src/articulated_system.cpp

@@ -102,13 +102,12 @@ void init_articulated_system(py::module &m) { // py::module &main_module) {
         .value("Cone", raisim::Shape::Type::Cone);
 
     /*******************/
-    /* Spring */
+    /* Spring          */
     /*******************/
     py::class_<raisim::ArticulatedSystem::SpringElement>(m, "SpringElement", "Spring element in an articulated system")
-        .def_readwrite("parentId", &raisim::CoordinateFrame::parentId)
-        .def_readwrite("parentName", &raisim::CoordinateFrame::parentName)
-        .def_readwrite("parentId", &raisim::CoordinateFrame::parentId);
-
+        .def_property("q_ref", &raisim::ArticulatedSystem::SpringElement::getSpringMount, &raisim::ArticulatedSystem::SpringElement::setSpringMount)
+        .def_readwrite("childBodyId", &raisim::ArticulatedSystem::SpringElement::childBodyId)
+        .def_readwrite("stiffness", &raisim::ArticulatedSystem::SpringElement::stiffness);
 
     /*******************/
     /* CoordinateFrame */
@@ -498,7 +497,7 @@ void init_articulated_system(py::module &m) { // py::module &main_module) {
          you have to use this with "void getPosition_W(size_t bodyIdx, const Vec<3> &point_B, Vec<3> &point_W)".
          If you want the orientation expressed in the world frame,
          you have to get the parent body orientation and pre-multiply it by the relative orientation*/
-        .def("getFrameByName", py::overload_cast<const std::string &>(&raisim::ArticulatedSystem::getFrameByName), R"mydelimiter(
+        .def("getFrameByName", py::overload_cast<const std::string &>(&raisim::ArticulatedSystem::getFrameByName), py::return_value_policy::reference, R"mydelimiter(
         Get the coordinate frame from its name.
 
         Args:
@@ -510,7 +509,7 @@ void init_articulated_system(py::module &m) { // py::module &main_module) {
         py::arg("name"))
 
 
-        .def("getFrameByIdx", py::overload_cast<size_t>(&raisim::ArticulatedSystem::getFrameByIdx), R"mydelimiter(
+        .def("getFrameByIdx", py::overload_cast<size_t>(&raisim::ArticulatedSystem::getFrameByIdx), py::return_value_policy::reference, R"mydelimiter(
         Get the coordinate frame from its index.
 
         Args:
@@ -521,14 +520,14 @@ void init_articulated_system(py::module &m) { // py::module &main_module) {
         )mydelimiter",
         py::arg("idx"))
 
-        .def("getSpring", &raisim::ArticulatedSystem::getSpring), R"mydelimiter(
+        .def("getSprings", py::overload_cast<>(&raisim::ArticulatedSystem::getSprings), py::return_value_policy::reference, R"mydelimiter(
         Get the spring elements.
 
         Returns:
             springs: list of spring elements.
         )mydelimiter")
 
-        .def("getFrameIdxByName", &raisim::ArticulatedSystem::getFrameIdxByName, R"mydelimiter(
+        .def("getFrameIdxByName", &raisim::ArticulatedSystem::getFrameIdxByName, py::return_value_policy::reference, R"mydelimiter(
         Get the coordinate frame index from its name.
 
         Args:
@@ -539,7 +538,7 @@ void init_articulated_system(py::module &m) { // py::module &main_module) {
         )mydelimiter",
         py::arg("name"))
 
-        .def("getFrames", py::overload_cast<>(&raisim::ArticulatedSystem::getFrames), R"mydelimiter(
+        .def("getFrames", py::overload_cast<>(&raisim::ArticulatedSystem::getFrames), py::return_value_policy::reference, R"mydelimiter(
         Get all the coordinate frames.
 
         Returns:
@@ -941,7 +940,7 @@ void init_articulated_system(py::module &m) { // py::module &main_module) {
 
 
 
-        .def("getMass", py::overload_cast<>(&raisim::ArticulatedSystem::getMass), R"mydelimiter(
+        .def("getMass", py::overload_cast<>(&raisim::ArticulatedSystem::getMass), py::return_value_policy::reference, R"mydelimiter(
         Return the body/link masses.
 
         Returns:

raisimPy/src/math.cpp

@@ -39,14 +39,6 @@ using namespace raisim;
 
 
 void init_math(py::module &m) {
-
-    /********/
-    /* Math */
-    /********/
-
-    // the code to convert between the data types is in `converter.hpp` and `converter.cpp`.
-
-
     /******************/
     /* Transformation */
     /******************/

thirdParty/pybind11/.clang-format

@@ -3,7 +3,6 @@
 # clang-format --style=llvm --dump-config
 BasedOnStyle: LLVM
 AccessModifierOffset: -4
-AlignConsecutiveAssignments: true
 AlwaysBreakTemplateDeclarations: Yes
 BinPackArguments: false
 BinPackParameters: false
@@ -15,7 +14,6 @@ IndentPPDirectives: AfterHash
 IndentWidth: 4
 Language: Cpp
 SpaceAfterCStyleCast: true
-# SpaceInEmptyBlock: true # too new
 Standard: Cpp11
 TabWidth: 4
 ...