add: FullPivot eigen solver, sensor traces, ChatGPT.py, many fixes: m…

…utable args, reevingSystemSprings, GetInterpolatedSignalValue, SolutionViewer, Minimize; see Issue tracker

README.rst

@@ -37,7 +37,7 @@ Exudyn
 
 **A flexible multibody dynamics systems simulation code with Python and C++**
 
-Exudyn version = 1.6.164.dev1 (Gillespie)
+Exudyn version = 1.7.0 (Hall)
 
 +  **University of Innsbruck**, Department of Mechatronics, Innsbruck, Austria
 

docs/RST/Abbreviations.rst

@@ -114,7 +114,7 @@ see also :ref:`Section Notation <sec-generalnotation>`\ .
 
 .. _T66:
 
-\ **T66**\ : Pl"ucker transformation
+\ **T66**\ : Plücker transformation
 
 .. _trig:
 

docs/RST/Examples/ROSExampleMassPoint.rst

@@ -0,0 +1,226 @@
+
+.. _examples-rosexamplemasspoint:
+
+**********************
+ROSExampleMassPoint.py
+**********************
+
+You can view and download this file on Github: `ROSExampleMassPoint.py <https://github.com/jgerstmayr/EXUDYN/tree/master/main/pythonDev/Examples/ROSExampleMassPoint.py>`_
+
+.. code-block:: python
+   :linenos:
+
+   #+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+   # This is an EXUDYN python example how to use ROS and EXUDYN
+   #
+   # Details:  This example shows how to communicate between an exudyn simulation
+   #            and ROS publisher and subscriber from bash
+   #           To make use of this example, you need to 
+   #           install ROS (ROS1 noetic) including rospy (see rosInterface.py)
+   #           prerequisits to use: 
+   #           use a bash terminal to start the roscore with: 
+   #               roscore 
+   #           send force command to add load to the mass point form bash file with: 
+   #               rostopic pub -r 100 /cmd_wrench geometry_msgs/WrenchStamped "..."
+   # Author:   Martin Sereinig, Peter Manzl 
+   # Date:     2023-05-31 (created)
+   #
+   # Copyright:This file is part of Exudyn. Exudyn is free software. 
+   # You can redistribute it and/or modify it under the terms of the Exudyn license. 
+   # See 'LICENSE.txt' for more details.
+   #
+   #+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+   import numpy as np
+   import exudyn as exu
+   from exudyn.utilities import *
+   
+   # import needed ROS modules and messages
+   import rospy
+   from geometry_msgs.msg import WrenchStamped, Twist
+   from std_msgs.msg import String
+   
+   # import new exudyn ROS interface class
+   import rosInterface as exuROS
+   
+   # here build inherited class and using within a simple exudyn simulation of one mass spring-damper 
+   class MyExudynROSInterface(exuROS.ROSInterface):
+       def __init__(self):
+           # use initialisation of super class
+           # this initialize a rosnode with name
+           super().__init__(name='exuROSexample3Dmass')
+           # initialization of all standard publisher done by super class
+           # self.exuPosePublisher
+           # self.exuStringPublisher
+           # self.exuSystemstatePublisher
+           # self.exuTimePublisher
+           # self.exuTwistPublisher
+           
+           # use standard publisher functions form super class!
+           # self.PublishPoseUpdate
+           # self.PublishTwistUpdate
+           # self.PublishSystemStateUpdate 
+   
+           # initialize all subscriber 
+           # suitable callback function is auto generated by superclass (using lambda function)
+           # twist subscriber: cmd_vel
+           twistSubsrciberBase = '' 
+           twistSubsrciberTopic = 'cmd_vel'     # topic to subscribe 
+           self.cmd_vel = Twist()              # data type of topic, must be named: self.twistSubscriberTopic
+           self.myTwistSubscriber = self.InitSubscriber(twistSubsrciberBase,twistSubsrciberTopic,Twist)
+   
+           # wrench subscriber: cmd_wrench
+           twistSubsrciberBase = '' 
+           twistSubsrciberTopic = 'cmd_wrench'     # topic to subscribe 
+           self.cmd_wrench = WrenchStamped()              # data type of topic, must be named: self.twistSubscriberTopic
+           self.myWrenchSubscriber = self.InitSubscriber(twistSubsrciberBase,twistSubsrciberTopic,WrenchStamped)
+   
+           # string subsriber: my_string
+           stringSubsrciberBase = ''
+           stringSubsrciberTopic = 'my_string'     # topic to subscribe 
+           self.my_string = String()              # data type of topic, must be named: self.twistSubscriberTopic
+           self.myStringSubscriber = self.InitSubscriber(stringSubsrciberBase,stringSubsrciberTopic,String)
+   
+            # string subsriber: /exudyn/SimpleString
+           stringSubsrciberBase2 = 'exudyn/' # namespace of topic 
+           stringSubsrciberTopic2 = 'SimpleString'     # topic to subscribe 
+           self.SimpleString = String()              # data type of topic, must be named: self.twistSubscriberTopic
+           self.myStringSubscriber = self.InitSubscriber(stringSubsrciberBase2,stringSubsrciberTopic2,String)       
+   
+   
+   # test main function 
+   def main():
+   
+       # build exudyn model 
+       SC = exu.SystemContainer()
+       mbs = SC.AddSystem()
+       tRes = 0.001    # step size in s
+       tEnd = 1e5    # simulation time in s
+       # mass and dimension of sphere
+       mass = 6
+       r = 0.03
+       background = GraphicsDataCheckerBoard(point=[-0.5,0,0], 
+                                           normal=[1, 0, 0], 
+                                           color=[0.7]*3+[1], 
+                                           alternatingColor=[0.8]*3+[1])
+   
+       graphicsSphere = GraphicsDataSphere(point=[0,0,0],
+                                       radius=r,
+                                       color=(1,0,0,1),
+                                       nTiles=64)
+   
+       origin = [0, 0, 0]
+       bGround = mbs.AddObject(ObjectGround(referencePosition=origin,
+                                               visualization=VObjectGround(graphicsData=[background])))
+   
+       inertiaSphere = InertiaSphere(mass=mass,radius=r)
+   
+       # user interaction point
+       [nUIP, bUIP]=AddRigidBody(mainSys = mbs, 
+                                   inertia = inertiaSphere, 
+                                   nodeType = str(exu.NodeType.RotationEulerParameters), 
+                                   position = [origin[0], origin[1], origin[2]], 
+                                   rotationMatrix = np.eye(3), 
+                                   angularVelocity = np.zeros(3),
+                                   velocity= [0,0,0],
+                                   gravity = [0, 0, 0], 
+                                   graphicsDataList = [graphicsSphere])
+   
+       # create markers:
+       mGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bGround, localPosition=[0, 0, 0.]))
+       mUIP = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bUIP))
+       mUIPLoad = mbs.AddLoad(LoadForceVector(markerNumber = mUIP,loadVector =[0,0,0]))
+   
+       k = 100
+       oSpringDamper = mbs.AddObject(RigidBodySpringDamper(markerNumbers=[mGround, mUIP],
+                                                           stiffness=np.eye(6)*k,
+                                                           damping=np.eye(6)*k*5e-2,
+                                                           visualization={'show': False, 'drawSize': -1, 'color': [-1]*4}))
+   
+   
+       # sensor for position of endpoint of pendulum
+       sensorPos = mbs.AddSensor(SensorBody(bodyNumber=bUIP,
+                                               outputVariableType=exu.OutputVariableType.Position))
+       sensorOri = mbs.AddSensor(SensorBody(bodyNumber=bUIP,
+                                               outputVariableType=exu.OutputVariableType.Rotation))
+       sensorVelt = mbs.AddSensor(SensorBody(bodyNumber=bUIP,
+                                               outputVariableType=exu.OutputVariableType.Velocity))
+       sensorVelr = mbs.AddSensor(SensorBody(bodyNumber=bUIP,
+                                               outputVariableType=exu.OutputVariableType.AngularVelocity))
+   
+       # store sensor value of each step in mbs variable, so that is accessible from user function
+       mbs.variables['pos'] = sensorPos
+       mbs.variables['ori'] = sensorOri
+       mbs.variables['velt'] = sensorVelt
+       mbs.variables['velr'] = sensorVelr
+   
+       # initialize ROS interface from own subclass
+       myROSInterface = MyExudynROSInterface()
+   
+       print('rosversion: ' + str(myROSInterface.myROSversionEnvInt))
+       rospy.logdebug('node running and publishing')
+   
+       # exudyn PreStepUserFunction
+       def PreStepUserFunction(mbs, t):
+           # send position data to ROS
+           myROSInterface.PublishPoseUpdate(mbs,t)
+           # send velocity data to ROS
+           myROSInterface.PublishTwistUpdate(mbs,t)
+           # send system state data to ROS
+           myROSInterface.PublishSystemStateUpdate(mbs,t)
+   
+           # get string data from ROS /my_string topic, please use: rostopic pub -r 100 /my_string geometry_msgs/WrenchStamped "..."
+           someMessage = myROSInterface.my_string.data
+           if someMessage != '' :
+               print('mystringdata',someMessage)
+           
+           # get wrench data from ROS /cmd_wrench topic, please use: rostopic pub -r 100 /cmd_wrench geometry_msgs/WrenchStamped "..."
+           rosForces = myROSInterface.cmd_wrench.wrench.force
+           rosTorques = myROSInterface.cmd_wrench.wrench.torque
+           print('forces from ROS:', rosForces)
+           print('torques from ROS : ', rosTorques)
+   
+           # demo set force to certain value received from ROS /cmd_wrench
+           newForce = [rosForces.x, rosForces.y, rosForces.z]
+           mbs.SetLoadParameter(mUIPLoad,'loadVector',newForce)
+   
+           return True
+       
+       mbs.SetPreStepUserFunction(PreStepUserFunction)
+       # assemble multi body system with all previous specified properties and components
+       mbs.Assemble()
+       # set simulation settings
+       simulationSettings = exu.SimulationSettings() #takes currently set values or default values
+       simulationSettings.timeIntegration.endTime = tEnd
+       simulationSettings.timeIntegration.numberOfSteps = int(tEnd/tRes)
+       simulationSettings.timeIntegration.newton.relativeTolerance = 1e-8*100
+       simulationSettings.timeIntegration.newton.absoluteTolerance = 1e-10
+       simulationSettings.timeIntegration.verboseMode = 1 # if 0 no output; higher --> more output information about solver
+       simulationSettings.timeIntegration.newton.useModifiedNewton = False
+       simulationSettings.timeIntegration.newton.numericalDifferentiation.minimumCoordinateSize = 1
+       simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5
+       simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations = False
+       simulationSettings.timeIntegration.simulateInRealtime = True    # crucial for operating with robot
+       simulationSettings.displayStatistics = True
+       simulationSettings.solutionSettings.solutionInformation = "3D Spring Damper"
+       simulationSettings.solutionSettings.writeSolutionToFile = False 
+       viewMatrix = np.eye(3)  @ RotationMatrixZ(np.pi/2)@ RotationMatrixX(np.pi/2)
+       SC.visualizationSettings.general.autoFitScene = False
+       # set up parameter for usage in WSL2 (Ubuntu 20.04) on Windows 10
+       SC.visualizationSettings.window.startupTimeout = 5000
+       SC.visualizationSettings.interactive.selectionLeftMouse=False
+       SC.visualizationSettings.interactive.selectionRightMouse=False 
+   
+       exu.StartRenderer(True)
+       exu.SolveDynamic(mbs, simulationSettings)
+   
+       return True
+   
+   # __main__ function for testing the interface 
+   if __name__ == "__main__":
+       try:
+           main()
+       except rospy.ROSInterruptException:
+           pass
+   
+
+