Merge pull request #183 from Steakkk/master

[WIP] Actualized actuators in pyomecaman

Author: pariterre

.travis.yml

@@ -2,7 +2,7 @@ language: cpp
 matrix:
   include:
   - os: linux
-    dist: bionic
+    dist: focal
     compiler: gcc
     env:
       - COMPILER=gcc
@@ -39,7 +39,7 @@ before_install:
     - if [[ "$TRAVIS_OS_NAME" == "linux" ]]; then 
         sudo add-apt-repository ppa:ubuntu-toolchain-r/test -y;
         sudo apt update;
-        sudo apt install gcc-6 g++-6 -y;
+        sudo apt install gcc g++ -y;
       fi
 
 install:

binding/python3/CMakeLists.txt

@@ -87,12 +87,8 @@ INSTALL(FILES "${CMAKE_CURRENT_BINARY_DIR}/${BIORBD_NAME}.py"
     DESTINATION "${Python3_SITELIB}/${BIORBD_NAME}"
 )
 
-INSTALL(FILES "${CMAKE_CURRENT_SOURCE_DIR}/__init__.py"
-    DESTINATION "${Python3_SITELIB}/${BIORBD_NAME}"
-)
-
-# Install the version file
-install(FILES "${CMAKE_CURRENT_SOURCE_DIR}/_version.py"
+file(GLOB PYTHON_FILES "${CMAKE_CURRENT_SOURCE_DIR}/*.py")
+INSTALL(FILES ${PYTHON_FILES}
     DESTINATION "${Python3_SITELIB}/${BIORBD_NAME}"
 )
 

binding/python3/__init__.py

@@ -1,9 +1,10 @@
 from .biorbd import *
 from ._version import __version__
-
+from .surface_max_torque_actuator import surface_max_torque_actuator
 
 if biorbd.currentLinearAlgebraBackend() == 1:
     from casadi import Function, MX
+
     def to_casadi_func(name, func, *all_param):
         mx_param = []
         for p in all_param:
@@ -14,4 +15,3 @@ def to_casadi_func(name, func, *all_param):
         if not isinstance(func_evaluated, MX):
             func_evaluated = func_evaluated.to_mx()
         return Function(name, mx_param, [func_evaluated]).expand()
-

binding/python3/surface_max_torque_actuator.py

@@ -0,0 +1,71 @@
+from .biorbd import currentLinearAlgebraBackend
+
+import numpy as np
+
+try:
+    from matplotlib import pyplot as plt
+
+    matplotlib_found = True
+except ModuleNotFoundError:
+    matplotlib_found = False
+if currentLinearAlgebraBackend() == 1:
+    from casadi import Function, MX
+
+
+def surface_max_torque_actuator(model, dof, resolution=40):
+    if not matplotlib_found:
+        raise ModuleNotFoundError("matplotlib must be installed to use biorbd.surface_max_torque_actuator")
+
+    d2r = np.pi / 180
+    min_bound_q = -200 * d2r
+    max_bound_q = 200 * d2r
+    min_bound_qdot = -500 * d2r
+    max_bound_qdot = 500 * d2r
+    nbq = model.nbQ()
+
+    if currentLinearAlgebraBackend() == 1:
+        torque_act = MX.sym("act", nbq, 1)
+        q_sym = MX.sym("q", nbq, 1)
+        qdot_sym = MX.sym("q_dot", nbq, 1)
+        torque_func = Function(
+            "torque_func",
+            [torque_act, q_sym, qdot_sym],
+            [model.torque(torque_act, q_sym, qdot_sym).to_mx()],
+            ["activation", "Q", "Qdot"],
+            ["Tau"],
+        )
+    else:
+        torque_func = model.torque
+
+    max_act = np.ones(nbq)
+    q = np.arange(min_bound_q, max_bound_q, (max_bound_q - min_bound_q) / resolution)
+    qdot = np.arange(min_bound_qdot, max_bound_qdot, (max_bound_qdot - min_bound_qdot) / resolution)
+
+    tau_pos = np.zeros((resolution, resolution))
+    tau_neg = np.zeros((resolution, resolution))
+    for i in range(resolution):
+        for j in range(resolution):
+            pos = torque_func(max_act, np.ones(nbq) * q[i], np.ones(nbq) * qdot[j])
+            neg = torque_func(-max_act, np.ones(nbq) * q[i], np.ones(nbq) * qdot[j])
+            if currentLinearAlgebraBackend() == 0:
+                pos = pos.to_array()
+                neg = neg.to_array()
+            tau_pos[i, j] = pos[dof]
+            tau_neg[i, j] = neg[dof]
+
+    q = q / d2r
+    qdot = qdot / d2r
+    q, qdot = np.meshgrid(qdot, q)
+
+    def plot_surface(tau):
+        fig = plt.figure()
+        ax = fig.gca(projection="3d")
+        ax.plot_surface(q, qdot, tau)
+        ax.set_xlabel("Qdot", fontsize=15)
+        ax.set_ylabel("Q", fontsize=15)
+        ax.set_zlabel("Tau", fontsize=15)
+
+    plot_surface(tau_pos)
+    plot_surface(tau_neg)
+
+    plt.show()