Merge branch 'Issue248_set_joints' into 'dev'

MbsData set variables type accept self-modified array

Closes #248

See merge request robotran/mbsysc!393

MBsysC/mbs_interface/MBsysPy/examples/7_constraints/userfctR/user_dirdyn.py

+# -*- coding: utf-8 -*-
+"""Module for the definition of functions related to Equilibrium analysis."""
+# Author: Robotran Team
+# (c) Universite catholique de Louvain, 2020
+
+
+def user_dirdyn_init(mbs_data, mbs_dirdyn):
+    """Run specific operation required by the user before running direct dynamic.
+
+    Parameters
+    ----------
+    mbs_data : MBsysPy.MbsData
+        The instance containing the multibody project.
+    mbs_dirdyn : MBsysPy.MbsDirdyn
+        The instance of the current direct dynamic process.
+
+    Returns
+    -------
+    None.
+
+    """
+    return
+
+
+def user_dirdyn_loop(mbs_data, mbs_dirdyn):
+    """Run specific operation required by the user at the end of each integrator step.
+
+    In case of multistep integrator, this function is not called at intermediate
+    steps.
+
+    Parameters
+    ----------
+    mbs_data : MBsysPy.MbsData
+        The instance containing the multibody project.
+    mbs_dirdyn : MBsysPy.MbsDirdyn
+        The instance of the current direct dynamic process.
+
+    Returns
+    -------
+    None.
+    """
+    return
+
+
+def user_dirdyn_finish(mbs_data, mbs_dirdyn):
+    """Run specific operations required by the user when direct dynamic analysis ends.
+
+    Parameters
+    ----------
+    mbs_data : MBsysPy.MbsData
+        The instance containing the multibody project.
+    mbs_dirdyn : MBsysPy.MbsDirdyn
+        The instance of the current direct dynamic process.
+
+    Returns
+    -------
+    None.
+
+    """
+    return

MBsysC/mbs_interface/MBsysPy/examples/7_constraints/workR/main.py

-
 # coding _*_ utf-8 _*
+"""Robotran tutorial project in Python, ""User constraints" part."""
+# author: Robotran Team
+# Copyright 2019 Universite Catholique de Louvain
 
-"""
-author: Thomas Gillis
-
-Created on 1/7/2013
-Last update on 25/11/2019
-
-Copyright 2019 Universite Catholique de Louvain
-"""
-#==============================================================================
+# =============================================================================
 # Packages loading
-#==============================================================================
-import sys, time
-sys.path.insert(1,"./../../../..") # Relative path for this example script
+# =============================================================================
+import sys
+import time
+# Add the relative default build of MBsysC folder (Linux/MacOs).
+sys.path.insert(1, "./../../../../../build/python")
 import MBsysPy as robotran
 
-#==============================================================================
+# =============================================================================
 # Project loading
-#==============================================================================
+# =============================================================================
 print("\n>>PYTHON>> Loading Project!")
-mbs_data = robotran.MbsData("../dataR/Userconstraints.mbs", 
+mbs_data = robotran.MbsData("../dataR/Userconstraints.mbs",
                             prj_functions_c=None, prj_lib_path="..")
 
-#==============================================================================
+# =============================================================================
 # Setting user constraint
-#==============================================================================
+# =============================================================================
 nUsrConst = 1
 mbs_data.set_nb_userc(nUsrConst)
 print(mbs_data)
 
-#==============================================================================
+# =============================================================================
 # Coordinate partitioning
-#==============================================================================
+# =============================================================================
 print("\n>>PYTHON>> Running partitioning!")
 mbs_part = robotran.MbsPart(mbs_data)
-mbs_part.set_options(rowperm = 1)
+mbs_part.set_options(rowperm=1)
 mbs_part.run()
 
-#==============================================================================
+# =============================================================================
 # Computation
-#==============================================================================
+# =============================================================================
 print("\n>>PYTHON>> Running direct dynamic!")
 mbs_dirdyn = robotran.MbsDirdyn(mbs_data)
 
 t0 = time.time()
 results = mbs_dirdyn.run()
 t1 = time.time()
-print("Computation time in second: "+ str(t1-t0))
+print("Computation time in second: {:.1f}".format(t1 - t0))
 
-##==============================================================================
-## Plotting
-##==============================================================================
+# =============================================================================
+# Plotting
+# =============================================================================
 from matplotlib import pyplot as plt
 
 q = results.q
-plt.figure(mbs_data.mbs_name+str(" Q1"))
-plt.plot(q[:,0], q[:,1])
+plt.figure(mbs_data.mbs_name + str(" Q1"))
+plt.plot(q[:, 0], q[:, 1])
 plt.xlabel("time [s]")
 plt.ylabel("q1 [rad]")
-plt.xlim((0,5))
-plt.ylim((-0.1,0.25))
+plt.xlim((0, 5))
+plt.ylim((-0.1, 0.25))
 
-plt.figure(mbs_data.mbs_name+str(" Q2"))
-plt.plot(q[:,0], q[:,2])
+plt.figure(mbs_data.mbs_name + str(" Q2"))
+plt.plot(q[:, 0], q[:, 2])
 plt.xlabel("time [s]")
 plt.ylabel("q2 [m]")
-plt.xlim((0,5))
-plt.ylim((0.2,0.4))
+plt.xlim((0, 5))
+plt.ylim((0.2, 0.4))
 
-plt.show()
\ No newline at end of file
+plt.show()

MBsysC/mbs_interface/MBsysPy/mbsyspy/mbs_data.py

@@ -2110,7 +2110,10 @@ class MbsData(object):
 
         """
         if isinstance(indices, (list, np.ndarray)):
-            for index in indices:
+            # Local list to avoid self-modification (ie. set all driven joints as independent)
+            # Removing index 0 from list (default value in MbsData::qu and other).
+            local_indices = list(filter(lambda q_i: q_i != 0, indices))
+            for index in local_indices:
                 libutilities.mbs_set_qu(self.mbs_data_ptr, index)
         else:
             libutilities.mbs_set_qu(self.mbs_data_ptr, indices)
@@ -2125,7 +2128,10 @@ class MbsData(object):
             The ids of the joints to set as dependent.
         """
         if isinstance(indices, (list, np.ndarray)):
-            for index in indices:
+            # Local list to avoid self-modification (ie. set all independent to dependent)
+            # Removing index 0 from list (default value in MbsData::qu and other).
+            local_indices = list(filter(lambda q_i: q_i != 0, indices))
+            for index in local_indices:
                 libutilities.mbs_set_qv(self.mbs_data_ptr, index)
         else:
             libutilities.mbs_set_qv(self.mbs_data_ptr, indices)
@@ -2140,7 +2146,10 @@ class MbsData(object):
             The ids of the joints to set as driven.
         """
         if isinstance(indices, (list, np.ndarray)):
-            for index in indices:
+            # Local list to avoid self-modification (ie. set all independent to driven)
+            # Removing index 0 from list (default value in MbsData::qu and other).
+            local_indices = list(filter(lambda q_i: q_i != 0, indices))
+            for index in local_indices:
                 libutilities.mbs_set_qdriven(self.mbs_data_ptr, index)
         else:
             libutilities.mbs_set_qdriven(self.mbs_data_ptr, indices)
@@ -2159,7 +2168,9 @@ class MbsData(object):
             The ids of the joints to set as actuated.
         """
         if isinstance(indices, (list, np.ndarray)):
-            for index in indices:
+            # Removing index 0 from list (default value in MbsData::qu and other).
+            local_indices = list(filter(lambda q_i: q_i != 0, indices))
+            for index in local_indices:
                 libutilities.mbs_set_qa(self.mbs_data_ptr, index)
         else:
             libutilities.mbs_set_qa(self.mbs_data_ptr, indices)
@@ -2174,7 +2185,10 @@ class MbsData(object):
             The ids of the joints to be removed from the actuated list.
         """
         if isinstance(indices, (list, np.ndarray)):
-            for index in indices:
+            # local list to avoid self-modification (ie. unset all qa)
+            # Removing index 0 from list (default value in MbsData::qu and other).
+            local_indices = list(filter(lambda q_i: q_i != 0, indices))
+            for index in local_indices:
                 libutilities.mbs_unset_qa(self.mbs_data_ptr, index)
         else:
             libutilities.mbs_unset_qa(self.mbs_data_ptr, indices)