Merge pull request #203 from pariterre/wrapping

Added cylinder in wrapping objects

Author: pariterre

CMakeLists.txt

@@ -3,7 +3,7 @@ if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.0")
     cmake_policy(SET CMP0042 NEW)
 endif()
 
-project(biorbd VERSION 1.4.4)
+project(biorbd VERSION 1.5.0)
 set (CMAKE_CXX_STANDARD 11)
 set (BIORBD_NAME ${PROJECT_NAME})
 

binding/biorbd_muscles.i.in

@@ -17,6 +17,8 @@
 #include "Muscles/State.h"
 #include "Muscles/StateDynamics.h"
 #include "Muscles/StateDynamicsBuchanan.h"
+#include "Muscles/WrappingObject.h"
+#include "Muscles/WrappingHalfCylinder.h"
 #ifdef MODULE_STATIC_OPTIM
 #include "Muscles/StaticOptimization.h"
 #endif
@@ -107,5 +109,7 @@
 %include "@CMAKE_SOURCE_DIR@/include/Muscles/State.h"
 %include "@CMAKE_SOURCE_DIR@/include/Muscles/StateDynamics.h"
 %include "@CMAKE_SOURCE_DIR@/include/Muscles/StateDynamicsBuchanan.h"
+%include "@CMAKE_SOURCE_DIR@/include/Muscles/WrappingObject.h"
+%include "@CMAKE_SOURCE_DIR@/include/Muscles/WrappingHalfCylinder.h"
 @SWIG_STATIC_OPTIMIZATION_INCLUDE_COMMAND@
 

binding/biorbd_utils.i.in

@@ -52,6 +52,12 @@ namespace std {
     }
 }
 
+%extend biorbd::utils::Vector3d{
+    biorbd::utils::NODE_TYPE typeOfNode() const{
+        return $self->biorbd::utils::Node::typeOfNode();
+    }
+}
+
 %include "@CMAKE_SOURCE_DIR@/include/Utils/UtilsEnum.h"
 %include "@CMAKE_SOURCE_DIR@/include/Utils/String.h"
 %include "@CMAKE_SOURCE_DIR@/include/Utils/Path.h"

examples/CMakeLists.txt

@@ -14,6 +14,9 @@ endif()
 if (MODULE_KALMAN)
     list(APPEND EXAMPLE_FILES "inverseKinematicsKalmanExample.cpp")
 endif()
+if (MODULE_MUSCLES)
+    list(APPEND EXAMPLE_FILES "WrappingObjectsExample.cpp")
+endif()
 
 foreach(FILE ${EXAMPLE_FILES})
     # Get the name of the current file
@@ -43,5 +46,6 @@ endforeach()
 file(COPY
     ${CMAKE_CURRENT_SOURCE_DIR}/pyomecaman.bioMod
     ${CMAKE_CURRENT_SOURCE_DIR}/arm26.bioMod
+    ${CMAKE_CURRENT_SOURCE_DIR}/WrappingObjectExample.bioMod
     DESTINATION ${CMAKE_CURRENT_BINARY_DIR}
 )

examples/Test_longueur_wrapping.py

@@ -0,0 +1,87 @@
+import biorbd
+import math
+
+import matplotlib.pyplot as plt
+import numpy as np
+from mpl_toolkits.mplot3d import Axes3D
+
+# functions for cylinder rotation
+def cyl2cart(r, theta, z):
+    return (r*np.cos(theta), r*np.sin(theta), z)
+
+
+def roll(R, zi, zf, RT):
+    t = np.arange(0, 2 * np.pi + 0.1, 0.1)
+    z = np.array([zi, zf])
+    t, z = np.meshgrid(t, z)
+    p, q = t.shape
+    r = R * np.ones([p, q], float)
+    # cylindrical coordinates to Cartesian coordinate
+    x, y, z = cyl2cart(r, t, z)
+
+    # Euler rotation
+    rot = np.dot(
+     RT.rot().to_array(),
+     np.array([x.ravel(), y.ravel(), z.ravel()])
+    )
+
+    x_rt = rot[0, :].reshape(x.shape) + RTw.trans().to_array()[0]
+    y_rt = rot[1, :].reshape(y.shape) + RTw.trans().to_array()[1]
+    z_rt = rot[2, :].reshape(z.shape) + RTw.trans().to_array()[2]
+
+    return x_rt, y_rt, z_rt,
+
+# model
+model = biorbd.Model("WrappingObjectExample.bioMod")
+
+# wrapping RT matrix 
+RTw = biorbd.WrappingCylinder(
+    model.muscle(0).pathModifier().object(0)).RT(model, np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]))
+RTw_trans = RTw.transpose()
+
+# Point in global base
+Po = [0.264534159209888, 0.08668973339838759, 0.4024172540921048]
+Pi = [0.24035592212771023, 0.36899416468322876, 0.8650273094461659]
+Pi_wrap = [0.28997869688863276, 0.34739632020407846, 0.6636920365713757]
+Po_wrap = [0.29330355695193333, 0.3289556960274852, 0.627410425452938]
+
+# Plot the surface
+fig = plt.figure()
+ax = fig.add_subplot(111, projection='3d')
+x, y, z = roll(0.05, -0.2, 0.2, RTw)
+ax.plot_surface(x, y, z)
+plt.plot((Po[0], Po_wrap[0]), (Po[1], Po_wrap[1]), (Po[2], Po_wrap[2]), marker='o')
+plt.plot((Pi[0], Pi_wrap[0]), (Pi[1], Pi_wrap[1]), (Pi[2], Pi_wrap[2]), marker='o')
+
+# To set Axe3D to (almost) equal
+max_range = np.array([x.max()-x.min(), y.max()-y.min(), z.max()-z.min()]).max()
+Xb = 0.5*max_range*np.mgrid[-1:2:2, -1:2:2, -1:2:2][0].flatten() + 0.5*(x.max()+x.min())
+Yb = 0.5*max_range*np.mgrid[-1:2:2, -1:2:2, -1:2:2][1].flatten() + 0.5*(y.max()+y.min())
+Zb = 0.5*max_range*np.mgrid[-1:2:2, -1:2:2, -1:2:2][2].flatten() + 0.5*(z.max()+z.min())
+for xb, yb, zb in zip(Xb, Yb, Zb):
+    ax.plot([xb], [yb], [zb], 'w')
+plt.show()
+
+# Compute muscle length
+r = biorbd.WrappingCylinder(model.muscle(0).pathModifier().object(0)).radius()
+l_muscle_bd = model.muscle(0).musculoTendonLength(model, np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]))
+
+
+l_wt_arc = np.sqrt((Po_wrap[0]-Po[0])**2 + (Po_wrap[1]-Po[1])**2 + (Po_wrap[2]-Po[2])**2) + np.sqrt((Pi_wrap[0]-Pi[0])**2 + (Pi_wrap[1]-Pi[1])**2 + (Pi_wrap[2]-Pi[2])**2)
+
+# Pi_wrap and Po_wrap provide in wrapping base to compute arc length
+Pi_wrap = biorbd.Vector3d(0.28997869688863276, 0.34739632020407846, 0.6636920365713757)
+Pi_wrap.applyRT(RTw_trans)
+Pi_wrap = Pi_wrap.to_array()
+
+Po_wrap = biorbd.Vector3d(0.29330355695193333, 0.3289556960274852, 0.627410425452938)
+Po_wrap.applyRT(RTw_trans)
+Po_wrap = Po_wrap.to_array()
+
+arc = math.acos(
+    ((Po_wrap[0] * Pi_wrap[0]) + (Pi_wrap[1] * Po_wrap[1])) / (
+        math.sqrt(((Pi_wrap[1]**2)+(Pi_wrap[0]**2)) * ((Po_wrap[1]**2) + (Po_wrap[0]**2))))) * r
+
+l_arc = math.sqrt(arc**2 + (Po_wrap[2]-Pi_wrap[2])**2)
+
+l_m = l_wt_arc + l_arc 	# l_m = 0.582246096069153

examples/WrappingObjectExample.bioMod

@@ -0,0 +1,69 @@
+version 4
+segment Seg0
+    rt pi/10 pi/8 pi/6 xyz 0.1 0.2 0.3
+    rotations xyz
+    mass 40
+    inertia
+        1 0 0
+        0 1 0
+        0 0 1
+    com 0.05 0.1 0.15
+    mesh 0 0 0
+    mesh 0.1 0.2 0.3
+endsegment
+
+// Marker 0
+    marker marker_0
+        parent Seg0
+        position 0.1 0.2 0.3
+    endmarker
+    
+// Seg1
+segment Seg1
+    parent Seg0
+    rt pi/10 pi/8 pi/6 xyz 0.1 0.2 0.3
+    rotations   xyz
+    mass 1
+    inertia
+        1 0 0
+        0 1 0
+        0 0 0.1
+    com 0.05 0.1 0.15
+    mesh 0 0 0
+    mesh 0.1 0.2 0.3
+
+endsegment
+// Marker 1
+    marker marker_1
+        parent Seg1
+        position 0.1 0.2 0.3
+    endmarker
+
+musclegroup Seg02seg1
+	OriginParent		Seg0
+	InsertionParent		Seg1
+endmusclegroup 
+	muscle	line
+		Type 			hill
+		musclegroup 		Seg02seg1
+		OriginPosition		0.02 -0.1 0.2
+		InsertionPosition	0.08 0.2 0.1
+		optimalLength		0.8
+		maximalForce		3
+		tendonSlackLength 	0.2
+		pennationAngle		0.43633
+		PCSA			3.7
+		maxVelocity 		10
+	endmuscle
+
+		wrapping cyl1
+			parent Seg0
+			type cylinder
+           		RT pi/10 pi/8 pi/6 xyz 0.1 0.2 0.3
+			muscle line
+			musclegroup Seg02seg1
+			diameter 0.1
+			length 2
+			wrappingside 1
+		endwrapping
+		

examples/WrappingObjectsExample.cpp

@@ -0,0 +1,48 @@
+#include "biorbd.h"
+
+///
+/// \brief main Get and print the generalized acceleration of a model with wrapping objects
+/// \return Nothing
+///
+/// This examples shows how to
+///     1. Load a model with a wrapping object
+///     2. Position the model at a chosen position (Q), velocity (Qdot)
+///     3. Compute the generalized acceleration (Qddot) assuming a set of muscle activations
+///     4. Print them to the console
+///
+/// Please note that this example will work only with the Eigen backend
+///
+int main()
+{
+    // Load a predefined model
+    biorbd::Model model("WrappingObjectExample.bioMod");
+
+    // Choose a position/velocity to compute dynamics from
+    biorbd::rigidbody::GeneralizedCoordinates Q(model);
+    biorbd::rigidbody::GeneralizedVelocity Qdot(model);
+    Q = Q.setOnes()/10;
+    Qdot.setZero();
+
+    // Set all muscles to half of their maximal activation
+    std::vector<std::shared_ptr<biorbd::muscles::State>> states;
+    for (unsigned int i=0; i<model.nbMuscles(); ++i){
+        states.push_back(std::make_shared<biorbd::muscles::State>(0, 0.5));
+    }
+
+    // Proceed with the computation of joint torque from the muscles
+    auto Tau = model.muscularJointTorque(states, Q, Qdot);
+
+    // Compute the generalized accelerations using the Tau from muscles.
+    // Please note that in forward dynamics setting, it is usually advised to
+    // additionnal residual torques. You would add them here to Tau.
+    auto Qddot = model.ForwardDynamics(Q, Qdot, Tau);
+
+    // Print them to the console
+    std::cout << Qddot.transpose() << std::endl;
+
+    // As an extra, let's print the individual muscle forces
+    std::cout << model.muscleForces(states, Q, Qdot).transpose() << std::endl;
+
+
+    return 0;
+}

include/Muscles/HillThelenType.h

@@ -7,7 +7,8 @@
 namespace biorbd {
 namespace muscles {
 ///
-/// \brief Muscle of Hill type augmented by Thelen (https://simtk-confluence.stanford.edu/display/OpenSim/Thelen+2003+Muscle+Model)
+/// \brief Muscle of Hill type augmented by Thelen
+/// https://simtk-confluence.stanford.edu/display/OpenSim/Thelen+2003+Muscle+Model
 ///
 class BIORBD_API HillThelenType : public biorbd::muscles::HillType
 {

include/Muscles/PathModifiers.h

@@ -76,7 +76,7 @@ class BIORBD_API PathModifiers
     const biorbd::utils::Vector3d& object(unsigned int  idx) const; 
 
 protected:
-    std::shared_ptr<std::vector<biorbd::utils::Vector3d>> m_obj; ///< set of objects
+    std::shared_ptr<std::vector<std::shared_ptr<biorbd::utils::Vector3d>>> m_obj; ///< set of objects
     std::shared_ptr<unsigned int> m_nbWraps; ///< Number of wrapping object in the set
     std::shared_ptr<unsigned int> m_nbVia; ///< Number of via points in the set
     std::shared_ptr<unsigned int> m_totalObjects; ///< Number of total objects in the set