diff --git a/.gitignore b/.gitignore index 8f2038d..ebd2af4 100644 --- a/.gitignore +++ b/.gitignore @@ -8,4 +8,6 @@ build/ install/ log/ *.view -*.json \ No newline at end of file +*.json +record_folder/ +*.txt diff --git a/README.md b/README.md index dae5857..b1f7661 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,249 @@ # prefab.MobileTrunk -The MobileTrunk's robot prefab for SoftRobots. +[![SoftRobots](https://img.shields.io/badge/info-on_website-purple.svg)](https://project.inria.fr/softrobot/) +[![SOFA](https://img.shields.io/badge/SOFA-on_github-blue.svg)](https://github.com/sofa-framework) +[![Ronotnik Automation](https://img.shields.io/badge/RonotnikAutomation-on_github-green.svg)](https://github.com/RobotnikAutomation) +[![Ros](https://img.shields.io/badge/Ros-on_readthedocs-chocolate.svg)](https://docs.ros.org/en/foxy/index.html) + +The *MobileTrunk's* robot prefab for SoftRobots. + +Index +----- + + * [Requirement](#requirement) + * [Compile sofa](#compile-sofa) + * [Install Required dependencies](#install-required-dependencies) + * [Download and set up working space](#download-and-set-up-working-space) + * [Install *ROS* and set up working space](#install-ros-and-set-up-working-space) + * [Launch test](#launch-test) + * [Connecting the digital twin to the physical robot](#connecting-the-digital-twin-to-the-physical-robot) + * [License](#license) + +Requirement +----------- +This prefab was developed as part of the SOMOROB project. The goal is to integrate a +deformable robotic trunk (softrobot) on a 4-wheeled mobile robot base equipped with +a LIDAR and to develop its digital twin equivalent. The deformable trunk is a project +[Echelon 3](https://www.inria.fr/en/interface-inria-centre-university-lille-demonstration-space) +developed by the Defrost team at INRIA. The mobile base is developed by the company +[Robotnik automation](https://robotnik.eu/) + +Software version +--------------- +The softwares are use with the following version : +- Python 3 + +The following Sofa plugins are required. version V21.12 is needed. + +- SofaPython3 +- BeamAdapter +- SoftRobots + +The following python module are required : + +- time +- threading +- math +- numpy +- dataclasses +- sys + +Compile *SOFA* +-------------- + +Install Required dependencies +------------------------------------ +For that follow these [instructions](https://www.sofa-framework.org/download/) (We will install the linux one) + + +Download and set up working space +--------------------------------- + +### Download the mobile trunk's prefab + +Now that you have install SOFA binairy we can clone +prefab.MobileTrunk repository. + +- Create a directory where you will put it +- Move into it and clone the last working version + + ```console + foo@bar:~$ git clone https://github.com/CRIStAL-PADR/prefab.MobileTrunk + ``` + +### Set up working space +Open your bashrc and add the following lines in order to setup your working space +- Add to the PATH the path to the folder where the binary files are located + + ```console + export PATH="/path to binary folder/bin:$PATH" + ``` + +- Tell to *SOFA_ROOT* the path where to find the path to the buid folder of SOFA + + ```console + export SOFA_ROOT=/path to binary folder + ``` + +- Make an alias in order to be able to launch Sofa easily by doing a runSofa + + ```console + runSofa="/path to binary folder/bin/runSofa" + ``` + +- Add to the PYTHONPATH the path to *SofaPython3*, *STLIB* , *SofaROS* , *Python3* + ```console + export PYTHONPATH=$PYTHONPATH:"/path to binary folder/plugin/SofaPython3/lib/python3/site-packages":"/path to binary folder/plugin/STLIB/lib/python3/site-packages":"/usr/local/lib/python3.8/dist-packages":"/path to binary folder/plugin/SoftRobots/share/sofa/examples/SoftRobots/sofaros" + ``` + +- Now you can source your .bashrc file with the command + ```console + source ~/.bashrc + ``` + +- And run the command: + ```console + runSofa + ``` +If everything went well, you should see the Sofa GUI appear. + +Now, click on the *Edit* button and go to *Plugin Manager*. Then click on *Add*. A window will appear. You need to navigate to the directory where the binary files are located. Go to the *plugins* directory, then to *ArticulatedSystemPlugin*, and then to *lib*. There, you will find a file named *libArticulatedSystemPlugin.so*. Click on *Open*. + +Repeat the same procedure to add the *.so* files for the *SofaPython3* plugin. + +Install *ROS* and set up working space +-------------------------------------- + +The digital twin has a *ros2* interface while the mobile platform (summit_xl) has a *ros1* interface. +It is possible to be able to link the digital twin to the real robot using rosbridge. + +### Install ros2 and ros1 + +Install the [noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) version for ros1 in order to be able to use rosbridge + +Install the [foxy](https://docs.ros.org/en/foxy/Installation.html) version for ros2. + +If you are new to ros you can follow the [tutorials](https://docs.ros.org/en/foxy/Tutorials.html) before continuing + +Install rosbridge + +```console + foo@bar:~$ sudo apt-get install -y ros-foxy-ros1-bridge +``` + +### Set up working space + +Open your bashrc and add the following lines in order to setup your working space for ros + +- Create an alias for noetic(ros1) and foxy(ros2) in order to be able to source your terminal more easily + + ```console + alias foxy="source /opt/ros/foxy/setup.bash" + alias noetic="source /opt/ros/noetic/setup.bash" + ``` + +- Open sofaros.py file located in plugings/SoftRobots/share/sofa/examples/SoftRobots folder. +Then replace in the callback method of RosReceiver class the line + ```console + self.data = data.data by self.data = data + ``` +Launch test +----------- +To confirm all the previous steps and verify that the prefab is working properly you can : + +#### Test using sofa controller + +- Launch the scene.py SOFA scene situated in prefab.MobileTrunk/mobile_trunk_sim by doing: + ```console + foo@bar:~$ runSofa scene.py --argv KeyboardController +``` +- If everything went well you should see the GUI Sofa with the digital twin of + + ![somorob](/docs/somorob.png) + + +- Then with your keyboard send velocity and orientation command to the Sofa scene in order to see the robot +move. + + +#### Test using ros api + +- source your terminal using : + ```console + foo@bar:~$ foxy + ``` + +- Go to the directory where the prefab.MobleTrunk/mobile_trunk_sim folder is located and run + the following command: + + ```console + foo@bar:~$ runSofa scene.py --argv RobotToSim + ``` +- Open a new terminal then source it as before then execute the commands to the following command: + + ```console + foo@bar:~$ colcon build && . install/setup.bash && foxy + ``` + +- You can now launch the keyboard controller to interact with the robot in the simulation + + ```console + foo@bar:~$ ros2 run summitxl summitxl_teleop_key + + ``` + +- If everything went well you should see this appear in the terminal + + ![ros_teleop_key](/docs/ros_teleopkey.png/) + + +## Echelon + +### Controller + +#### Forward kinematics + +- ctl + i : To select the cable i (from 1 to 9) you want to actuate +- ctl + plus : if you want to pull the cable +- ctl + minus : if you want to release the cable + +### Links + +You can found the Echelon3 folder with the reference simulation there : https://gitlab.inria.fr/defrost/Robots/Echelon3.git with the branch Centrale_Dynamixel. The reference simulation is in the folder **simulation**. + +There is also the ros2 package : https://gitlab.inria.fr/defrost/Robots/sim_echelon3_ros.git with the branch ros2. + +You can find all the details and the explanation in this two repository. + +Connecting the digital twin to the physical robot +------------------------------------------------- + + +License +------- + +Creative Commons Zero v1.0 Universal see LICENSE diff --git a/docs/SOMOROB test validation.pdf b/docs/SOMOROB test validation.pdf new file mode 100644 index 0000000..0f7ba5a Binary files /dev/null and b/docs/SOMOROB test validation.pdf differ diff --git a/docs/prefab.MoblieTrunkPresentation.pdf b/docs/prefab.MoblieTrunkPresentation.pdf new file mode 100644 index 0000000..1da3184 Binary files /dev/null and b/docs/prefab.MoblieTrunkPresentation.pdf differ diff --git a/docs/ros_teleopkey.png b/docs/ros_teleopkey.png new file mode 100644 index 0000000..446f9c6 Binary files /dev/null and b/docs/ros_teleopkey.png differ diff --git a/docs/sofa.png b/docs/sofa.png new file mode 100644 index 0000000..f6727f9 Binary files /dev/null and b/docs/sofa.png differ diff --git a/docs/somorob.png b/docs/somorob.png new file mode 100644 index 0000000..bb36de2 Binary files /dev/null and b/docs/somorob.png differ diff --git a/mobile_trunk_sim/addCamera.py b/mobile_trunk_sim/addCamera.py new file mode 100644 index 0000000..06d6f3c --- /dev/null +++ b/mobile_trunk_sim/addCamera.py @@ -0,0 +1,11 @@ +def addCamera(rootNode): + + camera = rootNode.addChild('Camera') + camera.addObject('MechanicalObject' , template='Rigid3d', rotation=[180, 0, 0],position=[1.13687e-13, 260, 1078.45, -0.5, -0.5, 0.5, 0.5], + showObject=False, showObjectScale=100) + + camera.addObject('RigidRigidMapping', name="mapping", input = rootNode.Trunk.framesNode.frames.getLinkPath() ,index=100) + + rootNode.getRoot().addObject('OglViewport', screenSize=[320, 200], name='Camera', zNear=5, + zFar=-10, fovy=55, + cameraRigid=camera.MechanicalObject.position.getLinkPath(), useFBO=False) \ No newline at end of file diff --git a/mobile_trunk_sim/echelon3/CableController.py b/mobile_trunk_sim/echelon3/CableController.py new file mode 100644 index 0000000..17aa9a9 --- /dev/null +++ b/mobile_trunk_sim/echelon3/CableController.py @@ -0,0 +1,55 @@ +import Sofa +import math + +class CableController(Sofa.Core.Controller): + """ + Controller to control the cables in forward kinematics + Parameters : + cables : list of the cables (constraint object) + name : the name of the component + """ + + def __init__(self, cables : list, name : str = "CableController"): + Sofa.Core.Controller.__init__(self, name = name) + + self.cables = cables + self.index =0 + # action at beginning frame + def onAnimateBeginEvent(self, eventType): + pass + + #action when key press + def onKeypressedEvent(self, c): + key = c['key'] + displacement = [math.inf] + + ##### cables number ###### + if (key == "1"): + self.index = 0 + elif (key == "2"): + self.index = 1 + elif (key == "3"): + self.index = 2 + elif (key == "4"): + self.index = 3 + elif (key == "5"): + self.index = 4 + elif (key == "6"): + self.index = 5 + elif (key == "7"): + self.index = 6 + elif (key == "8"): + self.index = 7 + elif (key == "9"): + self.index = 8 + + ##### add or substract ###### + if key == '+': + displacement = self.cables[self.index].value.value+1 + self.cables[self.index].value.value = [displacement] + print(f'Cables {self.index} displacement = {displacement[0]}') + + elif key == '-': + displacement = self.cables[self.index].value.value-1 + self.cables[self.index].value.value = [displacement] + print(f'Cables {self.index} displacement = {displacement[0]}') diff --git a/mobile_trunk_sim/echelon3/ForceController.py b/mobile_trunk_sim/echelon3/ForceController.py new file mode 100644 index 0000000..a5984d1 --- /dev/null +++ b/mobile_trunk_sim/echelon3/ForceController.py @@ -0,0 +1,61 @@ +import Sofa + + +class ForceController(Sofa.Core.Controller): + """ + Controller to control the cables in forward kinematics + Parameters : + mechanicalObject : the mechanicalObject to control + dt : step time as in Sofa Force(Sofa) = Force(N) *dt + name : the name of the component + """ + + def __init__(self, cables : list, dt : float, name : str = "CableController"): + Sofa.Core.Controller.__init__(self, name = name) + + self.cables = cables + self.forceMax = 1e20 # max 6kg + self.forceMin = 0 + self.dt = dt + + # action at beginning frame + def onAnimateBeginEvent(self, eventType): + pass + + #action when key press + def onKeypressedEvent(self, c): + key = c['key'] + + ##### cables number ###### + if (key == "1"): + self.index = 0 + elif (key == "2"): + self.index = 1 + elif (key == "3"): + self.index = 2 + elif (key == "4"): + self.index = 3 + elif (key == "5"): + self.index = 4 + elif (key == "6"): + self.index = 5 + elif (key == "7"): + self.index = 6 + elif (key == "8"): + self.index = 7 + elif (key == "9"): + self.index = 8 + + ##### add or substract ###### + if key == '+': + force = self.cables[self.index].value.value+981000*self.dt # F = g.mm.s-2 ==> 100g : 100*9810 = g.mm.s-2 + if force[0] <= self.forceMax : + self.cables[self.index].value.value = [force] + print(f'Cables {self.index} force = {force[0]*1e-6}') + + elif key == '-': + force = self.cables[self.index].value.value-981000*self.dt # F = g.mm.s-2 ==> 100g : 100*9810 = g.mm.s-2 + if force[0] >= -self.forceMin : + self.cables[self.index].value.value = [force] + print(f'Cables {self.index} force = {force[0]*1e-6}') + \ No newline at end of file diff --git a/mobile_trunk_sim/echelon3/MultiMappingConnection.py b/mobile_trunk_sim/echelon3/MultiMappingConnection.py new file mode 100644 index 0000000..b4b9c0e --- /dev/null +++ b/mobile_trunk_sim/echelon3/MultiMappingConnection.py @@ -0,0 +1,179 @@ + +from parameters import * +from classEchelon import * + +def createEchelon(echelon,target,parameters): + + + ######################################### + # adding Points and topology + ######################################### + + framesNode = echelon + framesNode.addObject('PointSetTopologyContainer', position=parameters.position); + frames = framesNode.addObject('MechanicalObject', name='frames', template='Rigid3d', showObject=1, showObjectScale=2, position=parameters.positionRigid) + + topoTubeNode=framesNode.addChild('topoTube') + topoTubeNode.addObject('EdgeSetTopologyContainer', edges=parameters.backboneEdges, name='edgeContainer') + topoTubeNode.addObject('EdgeSetTopologyModifier', name='edgeModif') + topoTubeNode.addObject('BeamInterpolation', name='TubeInterpolation', radius=parameters.sections[0].backbone.external_radius, innerRadius=parameters.sections[0].backbone.inner_radius, straight=1, defaultYoungModulus=parameters.sections[0].backbone.youngModulus, defaultPoissonRatio = 0.3) + topoTubeNode.addObject('AdaptiveBeamForceFieldAndMass', massDensity=parameters.sections[0].backbone.volume_mass) + + topoRibNode=framesNode.addChild('topoRibs') + topoRibNode.addObject('EdgeSetTopologyContainer', edges=parameters.ribsEdges,name='edgecont') + topoRibNode.addObject('BeamInterpolation',name = 'RibsInterpolation', crossSectionShape='rectangular', lengthY=parameters.sections[0].ribs[0].width, lengthZ=parameters.sections[0].ribs[0].thickness, straight=0, defaultYoungModulus=parameters.sections[0].ribs[0].youngModulus, DOF0TransformNode0= parameters.dof0TransformeNode0, DOF1TransformNode1 = parameters.dof1TransformeNode1, dofsAndBeamsAligned=0) + topoRibNode.addObject('AdaptiveBeamForceFieldAndMass', massDensity=parameters.sections[0].ribs[0].volumMass) + + ######################################### + # Cables + ######################################### + + # creation of the cables + + constraintPoints = framesNode.addChild('constraintPoints') + constraintPoints.addObject('MechanicalObject', position=parameters.positionConstraint) + test= constraintPoints.addObject('RigidMapping', rigidIndexPerPoint = parameters.rigidMapIndices, globalToLocalCoords=0) + test.init() + + if all_sections: + a = len(parameters.constraintIndices) + else : + a = parameters.numCables[0] + + for c in range(a): + if (inverse): + cable = constraintPoints.addObject('CableActuator', name='Section'+str(c//3+1)+'Cable'+str(c%3+1), indices=parameters.constraintIndices[c], hasPullPoint=0, maxForce=parameters.cables.forceMax, minForce=0,maxPositiveDisp=parameters.cables.stroke/2-parameters.cables.neutralPosition[c],maxNegativeDisp=parameters.cables.stroke/2+parameters.cables.neutralPosition[c]) + else: + cable = constraintPoints.addObject('CableConstraint', name ='Section'+str(c//3+1)+'Cable'+str(c%3+1), indices=parameters.constraintIndices[c], hasPullPoint=0, valueType=typeControl, value=0) + + ######################################## + # Effector constraint + ######################################### + + rotationParam=200; #150; + direction=[ 1,0,0,0,0,0, 0,1,0,0,0,0, 0,0,1,0,0,0, 0,0,0,rotationParam,0,0, 0,0,0,0,rotationParam,0, 0,0,0,0,0, rotationParam] + indice = parameters.numRibsTot+2*parameters.numRibs[0] + + if (inverse) : + if all_sections : + framesNode.addObject('PositionEffector', template='Rigid3', indices=parameters.backboneEdges[-1][-1], effectorGoal=target.position.linkpath, useDirections=[ 1, 1, 1, 0, 1, 1], directions=direction) + else : + framesNode.addObject('PositionEffector', template='Rigid3', indices=indice, effectorGoal=target.position.linkpath, useDirections=[ 1, 1, 1, 0, 1, 1], directions=direction) + + + return echelon + + + +def createScene(rootNode): + + ######################################### + # Plugins, data and Solvers + ######################################### + + rootNode.addObject('VisualStyle', displayFlags='hideBehaviorModels showForceFields showCollisionModels showInteractionForceFields'); + + rootNode.findData('dt').value= dt; + rootNode.findData('gravity').value= [0, 0, -9810]; + + rootNode.addObject('BackgroundSetting', color=[ 0, 0.16, 0.21]) + + rootNode.addObject('OglSceneFrame', style="Arrows", alignment="TopRight"); + rootNode.addObject('RequiredPlugin', name='SofaPython3') + rootNode.addObject('RequiredPlugin', name='BeamAdapter') + rootNode.addObject('RequiredPlugin', name='SoftRobots') + rootNode.addObject('RequiredPlugin', name='SoftRobots.Inverse') + rootNode.addObject('RequiredPlugin', name='EigenLinearSolvers') + rootNode.addObject('RequiredPlugin', name='SofaMeshCollision') + rootNode.addObject('RequiredPlugin', name='SofaPlugins', pluginName='SofaGeneralEngine SofaConstraint SofaImplicitOdeSolver SofaSparseSolver SofaDeformable SofaEngine SofaBoundaryCondition SofaRigid SofaTopologyMapping SofaOpenglVisual SofaGeneralAnimationLoop SofaMiscMapping') + + rootNode.addObject('FreeMotionAnimationLoop') + rootNode.addObject('DefaultVisualManagerLoop') + + if (inverse): + rootNode.addObject('QPInverseProblemSolver', name='QPSolver', printLog=0, epsilon=0.1, maxIterations=100, tolerance=1e-15) + else: + rootNode.addObject('GenericConstraintSolver', name='GSSolver', maxIterations=1000, tolerance=1e-15) + + + ######################################### + # grab points + # BLUE = consigne + # RED = polhemus racking (optionnal) + # GREEN = direct QP target + ######################################### + + blue_offset = 60 + blue_ray = 10 + green_offset = 40 + green_ray = 10 + + # GREEN = QP target + qpTargetNode= rootNode.addChild('QPTarget') + qpTarget = qpTargetNode.addObject('MechanicalObject', name='QPTarget', template='Rigid3d', position=sensor.start_pos, showObject=1) + # grabpoints + grabPointsTarget=qpTargetNode.addChild('grabPoints') + grabPointsTarget.addObject('MechanicalObject', name='grabPoints', template='Vec3', position=[[ green_offset, 0, 0], [ 0, green_offset, 0], [ 0, 0, green_offset]]) + grabPointsTarget.addObject('SphereCollisionModel', radius=green_ray, group=3) + grabPointsTarget.addObject('RigidMapping') + + # BLUE = consigne + consigneNode= rootNode.addChild('Consigne') + consigneNode.addObject('EulerImplicitSolver', firstOrder=1, rayleighStiffness=0.1) + consigneNode.addObject('CGLinearSolver', name='solver', iterations=100, tolerance = 1e-05, threshold = 1e-05) + consigne = consigneNode.addObject('MechanicalObject', name='Consigne', template='Rigid3d', position=sensor.start_pos, showObject=1) + consigneNode.addObject('UncoupledConstraintCorrection') + # grappoints + grabPointsCons=consigneNode.addChild('grab points') + polhemusVisual = grabPointsCons.addObject('MechanicalObject', name='PIGrabPoints', template='Vec3', position=[ [ 0, 0, 0], [ blue_offset, 0, 0], [ 0, blue_offset, 0], [ 0, 0, blue_offset]]) + grabPointsCons.addObject('SphereCollisionModel', radius=blue_ray, group=3) + grabPointsCons.addObject('RigidMapping') + + + ######################################## + # Create parameters + ######################################## + # + parameters = Echelon3([6,6,8],[12,11,7.375],[100,50.0*1.33, 35.0,35],[75,50.0,35, 35.0]) + # parameters = echelon3 = Echelon3([2],[5],[50.0*1.33, 35.0],[50.0, 35.0]) + # parameters = Echelon3([2,2],[8,8],[100.0,50.0*1.33, 35.0],[75.0,50.0, 35.0]) + + ######################################## + # Create Solvers + ######################################## + + simulation = rootNode.addChild('Simulation') + simulation.addObject('EulerImplicitSolver', rayleighStiffness=parameters.numerical.rayleighStiffness, rayleighMass=parameters.numerical.rayleighMass, vdamping=parameters.numerical.vdamping) + # simulation.addObject('EigenSimplicialLDLT', name='ldl', template='CompressedRowSparseMatrixMat3x3d'); + simulation.addObject('SparseLDLSolver', name='ldl', template='CompressedRowSparseMatrixMat3x3d'); + simulation.addObject('GenericConstraintCorrection' , solverName='ldl') + + ######################################## + # create Echelon + ######################################## + + echelonPoints = simulation.addChild('EchelonPoints') + positionRigid = parameters.positionRigid[0:parameters.backboneEdges[0][0]]+parameters.positionRigid[parameters.backboneEdges[0][1]:] + echelonPoints.addObject('MechanicalObject', name='frames', template='Rigid3d', showObject=1, showObjectScale=2, position=positionRigid) + + ######################################### + # Base + ######################################### + + BaseNode=simulation.addChild('Base') + BaseNode.addObject('MechanicalObject',name ='base', position=[0,0,0,0,0,0,1], template='Rigid3d') + BaseNode.addObject('RestShapeSpringsForceField', points=0, stiffness=1e10 , angularStiffness=1e15) + + ######################################## + # Arm + ######################################## + + echelon = BaseNode.addChild('Echelon') + echelonPoints.addChild(echelon) + createEchelon(echelon,qpTarget,parameters) + echelon.addObject('SubsetMultiMapping',template = 'Rigid3,Rigid3', input = [BaseNode.base.getLinkPath(),echelonPoints.frames.getLinkPath()], output =echelon.frames.getLinkPath() ,indexPairs = parameters.indexPairs) + + simulation.addObject('MechanicalMatrixMapper', template='Rigid3,Rigid3',object1 = BaseNode.base.getLinkPath(), object2 = echelonPoints.frames.getLinkPath(),nodeToParse = echelon.getLinkPath()) + + + return rootNode \ No newline at end of file diff --git a/mobile_trunk_sim/echelon3/RosCableController.py b/mobile_trunk_sim/echelon3/RosCableController.py new file mode 100644 index 0000000..ca7b1af --- /dev/null +++ b/mobile_trunk_sim/echelon3/RosCableController.py @@ -0,0 +1,80 @@ +# coding: utf8 +#!/usr/bin/env python3 +import Sofa +from std_msgs.msg import Float64MultiArray,Float64 + +def cable_displacement_recv(data, datafield): + + datafield.value = data.data + + +def end_effector_pos_send(data): + end_effector_pos = Float64MultiArray() + end_effector_pos.data = [] + data_list = [] + for value in data.value: + data_list.append(float(value)) + + end_effector_pos.data = data_list + + return end_effector_pos + + +class CableROSController(Sofa.Core.Controller): + def __init__(self, *args, **kwargs): + Sofa.Core.Controller.__init__(self, *args, **kwargs) + self.cables = kwargs["cables"] + self.robot = kwargs["robot"] + self.rosNode = kwargs["rosNode"] + self.robotToSim = kwargs["robotToSim"] + self.receiving_topic = None + self.index = 0 + self.Flag = None + + def get_index_by_rosNode(self): + + topic_name_format = ["/Inverse/Cable1/state/displacement", "/Inverse/Cable2/state/displacement", + "/Inverse/Cable3/state/displacement", "/Inverse/Cable4/state/displacement", + "/Inverse/Cable5/state/displacement", "/Inverse/Cable6/state/displacement", + "/Inverse/Cable7/state/displacement","/Inverse/Cable8/state/displacement", + "/Inverse/Cable9/state/displacement"] + #Get active topic name and type + topics_names_and_types = self.rosNode.get_topic_names_and_types() + + for topic_name, topic_type in topics_names_and_types: + #check the topic_type std_msgs/msg/Float64 to get corresponding topic_name + if topic_type ==['std_msgs/msg/Float64'] and topic_name in topic_name_format: + #get the list of publishers thanks to the get_publishers_info_by_topic method + publishers = self.rosNode.get_publishers_info_by_topic(topic_name) + if publishers: + self.Flag = False + # If publishers is True => whe have topic which is receiving message + self.receiving_topic = topic_name + topic_processing = self.receiving_topic.strip().split('/') + self.index = int(topic_processing[2].strip("Cable")) + break + + else: + self.Flag = True + + + return self.Flag + + + # action at beginning frame + def onAnimateBeginEvent(self, event): + + state = self.get_index_by_rosNode() + if not state: + #If node is active and receiving messages, state is False + with self.cables[self.index - 1].value.writeable() as t: + t[0] += [self.robot.effector_cable_data.value] + else: + #If no message is received, Echelon3 keeps its last position + with self.cables[self.index-1].value.writeable() as t: + t[0] = t[0] + + #Get the postion of the end of the Echelon3 + with self.robot.end_effector_pos.writeable() as end_effector_pos : + for i in range(0,7): + end_effector_pos[i] = self.robot.Chassis.AttachedArm.Trunk.framesNode.frames.position.value[100][i] \ No newline at end of file diff --git a/mobile_trunk_sim/echelon3/classEchelon.py b/mobile_trunk_sim/echelon3/classEchelon.py new file mode 100644 index 0000000..2377a07 --- /dev/null +++ b/mobile_trunk_sim/echelon3/classEchelon.py @@ -0,0 +1,291 @@ +from splib3.numerics import Quat + +from dataclasses import dataclass, field +from math import sin,cos,pi +import numpy as np + +@dataclass +class Numerical: + """ + Numerical parameters of the scene + """ + + # Euler Implicit Solver parameters + rayleighStiffness : float = 0.1 + rayleighMass : float = 1 + vdamping : float = 0 + + # num beam between ribs on the tube + num_beam_between_half_ribs : float = 1 + + # num beam for each rib + num_beam_on_rib : float = 1 + + # Rate Limiter Parameters + rise : list = field(default_factory=lambda :[100., 100., 100.]) + fall : list = field(default_factory=lambda :[-100., -100., -100.]) + +@dataclass +class Cables: + """ + Mechanical parameters for the cables + """ + + stroke : float = 145 + forceMax : float = 100*1e6 #10 N (1N = 1kg*m/s^2 = 1e6 g*mm/s^2) + # ribs parameters + neutralPosition : list = field(default_factory=lambda :[-40,0,0,0, 0,0,0, 20,-10,-10]) + dist : list = field(default_factory=lambda :[5,3,2]) + +@dataclass +class Base: + """ + Mechanical parameters for the Base, + The base (node 0) can move thanks to a sliding constraint + """ + maxSpeed : int = 20 #(20mm/s ?) + maxDisplacement : int = 140 # 14 cm in positive and negative direction = 24 cm in total + marginDisplacement : int = 10 # distance btwn end switch and minimal displacement + +@dataclass +class Rib: + + x_list : list # mm position in x of the first and last points of the ribs + height : list # mm heigth corresponds to the length (in the radial direction) of the ribs + angle : float # rad + + width : float = 14. # mm + thickness : float = 1.5 # mm + + volumMass : float = 0.001 # 0.001g /mm^3 + youngModulus : float = 1e9 #Pa (1Pa = 1N/m^2 = 1kg/(m*s^2) = 1g/(mm*s^2) + + nodes : list = field(default_factory=list) + + def __post_init__(self): + dx = (self.x_list[1]-self.x_list[0])/2 + + # describe with 3 points (two at the bottom and one on the top) + self.nodes = [[self.x_list[0],self.height[1],self.angle],[self.x_list[0]+dx,self.height[0],self.angle],[self.x_list[1],self.height[1],self.angle]] + + self.position = [ [n[0], n[1]*sin(n[2]), n[1]*cos(n[2])] for n in self.nodes ] + self.positionRigid =[ [ n[0], n[1]*sin(n[2]), n[1]*cos(n[2]), -sin(n[2]/2), 0, 0, cos(n[2]/2) ] for n in self.nodes ] + + self.createDof0TransformNode0([self.positionRigid[0][0],0,0,0,0,0,1],self.positionRigid[0]) + self.createDof1TransformNode1([self.positionRigid[-1][0],0,0,0,0,0,1],self.positionRigid[-1]) + + def createDof0TransformNode0(self,backbonePose,ribPose): + + # dof from the backbone to the rib + dof0_H_node0 = [0,0,0,0,-sin(pi/4),0,cos(pi/4)] + dof0_H_R0= self.inverseTransform(backbonePose) + dof0_H_dof0 = self.composeTransform(dof0_H_R0, ribPose) + dof0_H_node0 = self.composeTransform(dof0_H_dof0, dof0_H_node0) + self.dof0TransformeNode0 = [dof0_H_node0,[0.0, 0.0, 0.0, -0.0, 0, 0, 1.0]] + return dof0_H_node0 + + def createDof1TransformNode1(self,backbonePose,ribPose): + + dof1_H_node1 = [0,0,0,0,sin(pi/4),0,cos(pi/4)] + dof1_H_R0=self.inverseTransform(backbonePose) + dof1_H_dof1 = self.composeTransform(dof1_H_R0, ribPose) + dof1_H_node1 = self.composeTransform(dof1_H_dof1, dof1_H_node1) + self.dof1TransformeNode1 = [[0.0, 0.0, 0.0, -0.0, 0, 0, 1.0],dof1_H_node1] + return dof1_H_node1 + + def inverseTransform(self,a_H_b): + b_H_a = [] + a_H_b = list(np.float_(a_H_b)) + q = Quat([ a_H_b[3], a_H_b[4], a_H_b[5], a_H_b[6] ]) + b_H_a = b_H_a + list(q.rotate([ -a_H_b[0], -a_H_b[1],-a_H_b[2] ])) + q = q.getConjugate() + b_H_a = b_H_a + [q.take(0),q.take(1), q.take(2), q.take(3)] + + return b_H_a + + def composeTransform(self,a_H_b, b_H_c): + a_H_c = [] + a_H_b, b_H_c = list(np.float_(a_H_b)), list(np.float_(b_H_c)) + q1, q2 = Quat([a_H_b[3], a_H_b[4], a_H_b[5],a_H_b[6]]), Quat([b_H_c[3], b_H_c[4], b_H_c[5],b_H_c[6]]) + b_c_in_a = q1.getConjugate().rotate([ b_H_c[0], b_H_c[1], b_H_c[2]]) + a_H_c = a_H_c + [ a_H_b[0] + b_c_in_a[0], a_H_b[1] + b_c_in_a[1], a_H_b[2] + b_c_in_a[2] ] + q = Quat.product(q1,q2) + a_H_c = a_H_c + [q.take(0),q.take(1),q.take(2),q.take(3)] + + return a_H_c + +@dataclass +class Backbone: + """ + + """ + x_list : list # mm x position of the first and last points + numStep : int # number of points + + external_radius : int = 15 # mm + inner_radius : int = 14 # mm + thickness_support : int = 3 # mm + volume_mass : float = 0.0005 # 0.001g /mm^3 + youngModulus : float = 1e7 #Pa (1Pa = 1N/m^2 = 1kg/(m*s^2) = 1g/(mm*s^2) + + # create backbone points + def __post_init__(self): + + dx = (self.x_list[1]-self.x_list[0])/self.numStep + self.position = [[self.x_list[0]+i*dx,0,0] for i in range(self.numStep+1)] + self.positionRigid = [[self.x_list[0]+i*dx,0,0,0,0,0,1] for i in range(self.numStep+1)] + + +@dataclass +class Section : + """ + + """ + x_list : list #mm # first and last position on x axe + heigth : list #mm # heigth corresponds to the length (in the radial direction) of the first and last rib on the top part of the robot value linearly interpolated for each rib + side : list #mm # side corresponds to the length (in the radial direction) of the first and last rib on the other part of the robot value linearly interpolated for each rib + numRibs : int # number of ribs in this sections + numCables : int # number of cables in this sections + + def __post_init__(self): + + # deduce parameter + self.length = self.x_list[1]-self.x_list[0] #mm + self.numRibsTot = self.numRibs*self.numCables + self.angles = [ i*(2*pi/self.numCables) for i in range(self.numCables)] # in radian for each cable position + + # create backbone + self.backbone = Backbone(self.x_list,self.numRibs*2) + + # creates ribs + dh = (self.heigth[0]-self.heigth[1])/self.numRibs + ds = (self.side[0]-self.side[1])/self.numRibs + self.dx = (self.x_list[1] - self.x_list[0])/self.numRibs + + self.ribs = [] + for i in range(self.numRibs): + self.ribs = self.ribs + [ Rib([self.x_list[0]+i*self.dx,self.x_list[0]+(i+1)*self.dx],[self.heigth[0]-i*dh,self.backbone.external_radius],self.angles[0])] + for j in range(1,self.numCables): + self.ribs = self.ribs + [ Rib([self.x_list[0]+i*self.dx,self.x_list[0]+(i+1)*self.dx],[self.side[0]-i*ds,self.backbone.external_radius],self.angles[j])] + + + # get position + self.positionRibs = [self.ribs[i].position[1] for i in range(self.numRibsTot)] + self.positionRigidRibs = [self.ribs[i].positionRigid[1] for i in range(self.numRibsTot)] + + # get doftransform + self.dof0TransformeNode0 = [self.ribs[i].dof0TransformeNode0 for i in range(self.numRibsTot)] + self.dof1TransformeNode1 = [self.ribs[i].dof1TransformeNode1 for i in range(self.numRibsTot)] + + + +@dataclass +class Echelon3: + """ + Contains all the parameters for echelon3 and create the use points and transformation for the robot + """ + numRibs : list # defines the number of ribs per section + steps : list # lentht of ONE step of the backbone. ribs MUST align to them + + ribsHeight : list = field(default_factory=lambda :[100.0*1.33, 50.0, 35.0,35]) # in mm #height corresponds to the length (in the radial direction) of the ribs placed on the upper part of the robot + ribsSide : list = field(default_factory=lambda :[75.0, 50.0, 35.0, 35.0]) # in mm ribs side corresponds to the length (in the radial direction) of the ribs placed on the upper part of the robot + + step : float = 3.85 # mm : lentht of ONE step of the backbone. ribs MUST align to them + numCables : list = field(default_factory=lambda :[3,3,3]) # num of cables in each section + + cables : Cables = Cables() + base : Base = Base() + numerical : Numerical = Numerical() + + def __post_init__(self): + + # deduce parameters + self.numSections = len(self.numRibs) + self.x_list = [0] # beginning and end of each section + self.numRibsTot = sum([self.numRibs[i]*self.numCables[i] for i in range(self.numSections)]) + + for i in range(self.numSections) : + self.x_list = self.x_list + [self.x_list[i] + self.numRibs[i]*self.step*self.steps[i] ] + + # create sections + self.sections = [Section([self.x_list[i],self.x_list[i+1]],[self.ribsHeight[i],self.ribsHeight[i+1]],[self.ribsSide[i],self.ribsSide[i+1]],self.numRibs[i],self.numCables[i]) for i in range(self.numSections)] + + # get positions + positionRibs,positionRigidRibs,positionBackbone,positionRigidBackbone = [],[],[],[] + for i in range(self.numSections): + positionRibs.extend(self.sections[i].positionRibs) + positionRigidRibs.extend(self.sections[i].positionRigidRibs) + positionBackbone.extend(self.sections[i].backbone.position[int(i>0):]) + positionRigidBackbone.extend(self.sections[i].backbone.positionRigid[int(i>0):]) + + self.position = positionRibs + positionBackbone + self.positionRigid = positionRigidRibs + positionRigidBackbone + + # get dof trasnform + self.dof0TransformeNode0 = [self.sections[i].dof0TransformeNode0 for i in range(self.numSections)] + self.dof1TransformeNode1 = [self.sections[i].dof1TransformeNode1 for i in range(self.numSections)] + + # create Edges + self.backboneEdges = [[self.numRibsTot+i,self.numRibsTot+i+1] for i in range(2*sum(self.numRibs))] + self.ribsEdges = [] + rib,ribpass = 0,0 + for s in range(self.numSections): + for r in range(self.numRibs[s]): + for c in range(self.numCables[s]): + self.ribsEdges.extend([2*ribpass+self.numRibsTot,rib,rib,2*(ribpass+1)+self.numRibsTot]) + rib+=1 + ribpass +=1 + + + # add actuator + # rigidMapIndices: for each constraint pos, provides the indice of the input NEW node + self.rigidMapIndices = [self.backboneEdges[0][0] for i in range(self.numCables[0]+1)] + for i in range(self.numRibsTot): + self.rigidMapIndices.append(i) + + #posConstraint: position of the constraint point + self.positionConstraint = [[0,0,0],[0, 0, self.ribsHeight[0]+ self.cables.dist[0] ] ] + + # position of the constraint were we place Cable Attachement on the First Rib + for i in range(1,self.numCables[0]): + self.positionConstraint.append([ 0, (self.ribsSide[0]+self.cables.dist[0])*sin(i*2*pi/self.numCables[0]), (self.ribsSide[0]+self.cables.dist[0])*cos(i*2*pi/self.numCables[0])]) + # these constraint are attached to node 0 (as the 3 nodes attached to the ground were fixed, see below) + + it_ribs=0; + it_point=self.numCables[0] + + self.constraintIndices = [ ] + for s in range(self.numSections): + # n cables created for each section, we need to store the indices of the constraint point + listOfIndices= [ [it_point-j] for j in range(self.numCables[s]-1,-1,-1)] + + for r in range(self.numRibs[s]): #iterates on num_ribs + it_ribs = it_ribs +1; + it_point = it_point+self.numCables[s] + for i in range(self.numCables[s]): + self.positionConstraint = self.positionConstraint + [[0, 0, self.cables.dist[s]]] + listOfIndices[i] = listOfIndices[i] + [it_point-(self.numCables[s]-i-1)] + + self.constraintIndices = self.constraintIndices + listOfIndices + + # create indexPairs + self.indexPairs = [] + for i in range(self.numRibsTot): + self.indexPairs.extend([1,i]) + self.indexPairs.extend([0,0]) + for edges in self.backboneEdges[1:]: + self.indexPairs.extend([1,edges[0]-1]) + self.indexPairs.extend([1,self.backboneEdges[-1][-1]-1]) + + + + + +# echelon3 = Echelon3([6,6,8,7],[25,11,7,6],[200,100,50.0*1.33, 35.0,35],[100,75,50.0,35, 35.0]) + +# echelon3 = Echelon3([6,6,8],[12,11,7],[100,50.0*1.33, 35.0,35],[75,50.0,35, 35.0]) +# echelon3 = Echelon3([2,2],[8,8],[100.0,50.0*1.33, 35.0],[75.0,50.0, 35.0]) +# echelon3 = Echelon3([2],[5],[50.0*1.33, 35.0],[50.0, 35.0]) + + # echelon3.position[numSection][0 : ribs, 1 : backbone][numRibs][point rib] + # echelon3.position[].ribs[numribs][0 : first point (for first rib or middle point for other), 1: middle of last point] diff --git a/mobile_trunk_sim/echelon3/createEchelon.py b/mobile_trunk_sim/echelon3/createEchelon.py new file mode 100644 index 0000000..2bb9a96 --- /dev/null +++ b/mobile_trunk_sim/echelon3/createEchelon.py @@ -0,0 +1,110 @@ +from .parameters import * +from .classEchelon import * + +if not inverse : + from .CableController import * + from .ForceController import * + + +def createEchelon(echelon,base,index,translation,rotation): + """ + Creates topology and cables for echelon3 arm + Parameters : + echelon : Sofa child on which we add everything + base : mechanical object on which we attach the arm + index : index of the mechanical point we attach the arm + translation : from 0,0,0 where we creat the arm + rotation : from 0,0,0 where we creat the arm + """ + + parameters = Echelon3([6,6,8],[12,11,7.375],[100,50.0*1.33, 35.0,35],[75,50.0,35, 35.0]) + + position = echelon.addObject('TransformEngine',name="transform" ,template="Vec3d" ,translation=translation ,rotation=rotation ,input_position=parameters.position) + positionRigid = echelon.addObject('TransformEngine',name="transformRigid" ,template="Rigid3d" ,translation=translation ,rotation=rotation ,input_position=parameters.positionRigid) + # position = echelon.addObject('TransformEngine',name="transform" ,template="Vec3d" ,translation=[0,0,0] ,rotation=[45,0,0] ,input_position=position.output_position) + # positionRigid = echelon.addObject('TransformEngine',name="transformRigid" ,template="Rigid3d" ,translation=[0,0,0] ,rotation=[45,0,0] ,input_position=positionRigid.output_position) + position.init() + positionRigid.init() + + ######################################### + # adding Points and topology + ######################################### + + framesNode = echelon.addChild('framesNode') + + framesNode.addObject('PointSetTopologyContainer', position=position.output_position); + frames = framesNode.addObject('MechanicalObject', name='frames', template='Rigid3d', showObject=1, showObjectScale=2, position=positionRigid.output_position) + + topoTubeNode=framesNode.addChild('topoTube') + topoTubeNode.addObject('EdgeSetTopologyContainer', edges=parameters.backboneEdges, name='edgeContainer') + topoTubeNode.addObject('EdgeSetTopologyModifier', name='edgeModif') + topoTubeNode.addObject('BeamInterpolation', name='TubeInterpolation', radius=parameters.sections[0].backbone.external_radius, innerRadius=parameters.sections[0].backbone.inner_radius, straight=1, defaultYoungModulus=parameters.sections[0].backbone.youngModulus, defaultPoissonRatio = 0.3) + topoTubeNode.addObject('AdaptiveBeamForceFieldAndMass', massDensity=parameters.sections[0].backbone.volume_mass) + + topoRibNode=framesNode.addChild('topoRibs') + topoRibNode.addObject('EdgeSetTopologyContainer', edges=parameters.ribsEdges,name='edgecont') + topoRibNode.addObject('BeamInterpolation',name = 'RibsInterpolation', crossSectionShape='rectangular', lengthY=parameters.sections[0].ribs[0].width, lengthZ=parameters.sections[0].ribs[0].thickness, straight=0, defaultYoungModulus=parameters.sections[0].ribs[0].youngModulus, DOF0TransformNode0= parameters.dof0TransformeNode0, DOF1TransformNode1 = parameters.dof1TransformeNode1, dofsAndBeamsAligned=0) + topoRibNode.addObject('AdaptiveBeamForceFieldAndMass', massDensity=parameters.sections[0].ribs[0].volumMass) + + ######################################### + # Spring to connect the arm to a point + ######################################### + + framesNode.addObject('RestShapeSpringsForceField', points=parameters.backboneEdges[0][0], external_points = index,external_rest_shape =base.getLinkPath() ,stiffness=1e15 , angularStiffness=1e20) + + ######################################### + # Cables + ######################################### + + # creation of the cables + + constraintPoints = framesNode.addChild('constraintPoints') + constraintPoints.addObject('MechanicalObject', position=parameters.positionConstraint) + test= constraintPoints.addObject('RigidMapping', rigidIndexPerPoint = parameters.rigidMapIndices, globalToLocalCoords=0) + test.init() + + cables = [] + for c in range(len(parameters.constraintIndices)): + cable = constraintPoints.addObject('CableConstraint', name ='Section'+str(c//3+1)+'Cable'+str(c%3+1), indices=parameters.constraintIndices[c], hasPullPoint=0, valueType=typeControl, value=0) + cables.append(cable) + + ########################################### + # # Mechanical model for colision + # ########################################## + trunk = framesNode.addChild("Trunk") + + trunk.addObject("MechanicalObject", name="position", template="Vec3d", + position=position.output_position, showObject=True) + trunk.addObject('UniformMass', totalMass=0.032) + + # trunk.addObject('TetrahedronHyperelasticityFEMForceField', + # name="linearElasticBehavior", + # youngModulus=250, + # poissonRatio=0.45) + + ########################################### + # # Visual model for colision + # ########################################## + # visual = trunk.addChild("VisualModel") + # visual.addObject('MeshSTLLoader', name='loader', filename='meshes/model_colision.stl',scale3d = [1000,1000,1000], rotation=[90,90,180], + # translation=[0., 0.26*1000, 0.65*1000]) + + # visual.addObject('MeshTopology', src='@loader') + # visual.addObject('OglModel', name="renderer", src='@loader', color=[0.2,0.2,0.2,1.0]) + # visual.addObject('SkinningMapping' , template="Rigid3d,Vec3d", input=frames.getLinkPath()) + + # ########################################## + # # Colision model + # ########################################## + + # collison_model = trunk.addChild("CollisionModel") + # collison_model.addObject('MeshSTLLoader', name='loader1', filename='meshes/model_colision.stl', scale3d=[1000, 1000, 1000],rotation=[90,90,180], + # translation=[0., 0.26*1000, 0.65*1000]) + # collison_model.addObject('MeshTopology', src='@loader1') + # collison_model.addObject('MechanicalObject', src = '@loader1') + # collison_model.addObject('TriangleCollisionModel', group=0) + # collison_model.addObject('LineCollisionModel',group=0) + # collison_model.addObject('PointCollisionModel', group=0) + # collison_model.addObject('SkinningMapping' , template="Rigid3d,Vec3d", input=frames.getLinkPath()) + + return parameters,cables \ No newline at end of file diff --git a/mobile_trunk_sim/echelon3/echelon3.py b/mobile_trunk_sim/echelon3/echelon3.py new file mode 100644 index 0000000..e994cf3 --- /dev/null +++ b/mobile_trunk_sim/echelon3/echelon3.py @@ -0,0 +1,59 @@ +from createEchelon import * + +from parameters import * + +from ForceController import * + + +def createScene(rootNode): + + ######################################### + # Plugins, data and Solvers + ######################################### + + rootNode.addObject('VisualStyle', displayFlags='hideBehaviorModels showForceFields showCollisionModels showInteractionForceFields'); + + rootNode.findData('dt').value= dt; + rootNode.findData('gravity').value= [0, 0, -9810]; + + rootNode.addObject('BackgroundSetting', color=[ 0, 0.16, 0.21]) + + rootNode.addObject('OglSceneFrame', style="Arrows", alignment="TopRight"); + rootNode.addObject('RequiredPlugin', name='SofaPython3') + rootNode.addObject('RequiredPlugin', name='BeamAdapter') + rootNode.addObject('RequiredPlugin', name='SoftRobots') + rootNode.addObject('RequiredPlugin', name='SoftRobots.Inverse') + rootNode.addObject('RequiredPlugin', name='EigenLinearSolvers') + rootNode.addObject('RequiredPlugin', name='SofaMeshCollision') + rootNode.addObject('RequiredPlugin', name='SofaPlugins', pluginName='SofaGeneralEngine SofaConstraint SofaImplicitOdeSolver SofaSparseSolver SofaDeformable SofaEngine SofaBoundaryCondition SofaRigid SofaTopologyMapping SofaOpenglVisual') + + rootNode.addObject('FreeMotionAnimationLoop') + rootNode.addObject('DefaultVisualManagerLoop') + + rootNode.addObject('GenericConstraintSolver', name='GSSolver', maxIterations=1000, tolerance=1e-15) + + ######################################### + # Base + ######################################### + + BaseNode=rootNode.addChild('Base') + base = BaseNode.addObject('MechanicalObject',name ='base', position=[-100,0,0,0,0,0,1], template='Rigid3d') + BaseNode.addObject('RestShapeSpringsForceField', points=0, stiffness=1e10 , angularStiffness=1e15) + + ######################################## + # create Echelon + ######################################## + + echelon = BaseNode.addChild('Echelon') + parameters = createEchelon(echelon,base,[-100,0,0],[0,0,0]) + + ######################################### + # Controllers + ######################################### + + if typeControl == 'force': + echelon.addObject(ForceController(echelon.framesNode.constraintPoints,dt, name = 'Cablecontroller')) + else : + echelon.addObject(CableController(echelon.framesNode.constraintPoints, name = 'Cablecontroller')) + + return rootNode \ No newline at end of file diff --git a/mobile_trunk_sim/echelon3/parameters.py b/mobile_trunk_sim/echelon3/parameters.py new file mode 100644 index 0000000..73d84ac --- /dev/null +++ b/mobile_trunk_sim/echelon3/parameters.py @@ -0,0 +1,36 @@ +from .classEchelon import * + +all_sections = True + +# time step +dt = 0.01 + +##################### +### type of simulation +##################### + +inverse = False # inverse model +base_translation = True +rate_limiter = True # limit the movement of the target +typeControl = 'displacement' + +ros = False + +########### Sensor ############# + +@dataclass +class Sensor: + + position : float + offset : float + axe : int = 0 + + def __post_init__(self): + self.start_pos = [0,0,0, 0,0,0,1] + self.start_pos[self.axe] = self.position-self.offset + +# create sensor +if all_sections : + sensor = Sensor(760,50,0) +else : + sensor = Sensor(260,10,0) \ No newline at end of file diff --git a/mobile_trunk_sim/floor.py b/mobile_trunk_sim/floor.py new file mode 100644 index 0000000..daa7c55 --- /dev/null +++ b/mobile_trunk_sim/floor.py @@ -0,0 +1,8 @@ +def Floor(parentNode, name, translation, color=[0.5, 0.5, 0.5, 1.]): + + floor = parentNode.addChild(name) + floor.addObject('MeshSTLLoader', name='loader', filename='meshes/Assembly.stl', + rotation=[-270, 0, 90],scale=2000, translation=translation ) + floor.addObject('MeshTopology', src='@loader') + floor.addObject('OglModel', name="renderer", src='@loader', color=color) + floor.addObject('MechanicalObject') \ No newline at end of file diff --git a/mobile_trunk_sim/functional_test/test.py b/mobile_trunk_sim/functional_test/test.py new file mode 100644 index 0000000..e904ad0 --- /dev/null +++ b/mobile_trunk_sim/functional_test/test.py @@ -0,0 +1,24 @@ +from wheels_angles_compute import twistToWheelsAngularSpeed, move + +elapsed_time = 0 +deplacement_ctrl = 0 +final_pos = 0 + +def test_move(angular_speed, linear_speed, duration, dt, robot_base_pos, WheelsMotors_angles_rest_position): + print("dt = ", elapsed_time) + elapsed_time +=dt + elapsed_time +=dt + deplacement = linear_speed * dt + deplacement_ctrl +=deplacement + + if elapsed_time >= duration: + angular_speed = 0 + linear_speed = 0 + final_pos =[robot_base_pos.value[0][0], + robot_base_pos.value[0][1], + robot_base_pos.value[0][2]] + print("final position = ", final_pos, "traveled distance = ", deplacement_ctrl) + + else : + wheels_angular_speed = twistToWheelsAngularSpeed(angular_speed, linear_speed) + move(WheelsMotors_angles_rest_position, wheels_angular_speed, dt) \ No newline at end of file diff --git a/mobile_trunk_sim/meshes/Assembly.stl b/mobile_trunk_sim/meshes/Assembly.stl new file mode 100644 index 0000000..1ff6ab5 Binary files /dev/null and b/mobile_trunk_sim/meshes/Assembly.stl differ diff --git a/mobile_trunk_sim/meshes/collision_wheel.stl b/mobile_trunk_sim/meshes/collision_wheel.stl new file mode 100644 index 0000000..0a0b6fa --- /dev/null +++ b/mobile_trunk_sim/meshes/collision_wheel.stl @@ -0,0 +1,1234 @@ +solid ASCII + facet normal -9.396926e-01 3.420201e-01 0.000000e+00 + outer loop + vertex 3.830222e-02 -3.213938e-02 -3.000000e-02 + vertex 5.000000e-02 0.000000e+00 7.000000e-02 + vertex 5.000000e-02 0.000000e+00 -3.000000e-02 + endloop + endfacet + facet normal -9.396926e-01 -3.420201e-01 0.000000e+00 + outer loop + vertex 5.000000e-02 0.000000e+00 -3.000000e-02 + vertex 5.000000e-02 0.000000e+00 7.000000e-02 + vertex 3.830222e-02 3.213938e-02 7.000000e-02 + endloop + endfacet + facet normal -9.396926e-01 -3.420201e-01 -9.509985e-17 + outer loop + vertex 5.000000e-02 0.000000e+00 -3.000000e-02 + vertex 3.830222e-02 3.213938e-02 7.000000e-02 + vertex 3.830222e-02 3.213938e-02 -3.000000e-02 + endloop + endfacet + facet normal -5.000000e-01 -8.660254e-01 -9.296355e-18 + outer loop + vertex 3.830222e-02 3.213938e-02 -3.000000e-02 + vertex 3.830222e-02 3.213938e-02 7.000000e-02 + vertex 8.682409e-03 4.924039e-02 7.000000e-02 + endloop + endfacet + facet normal -5.000000e-01 -8.660254e-01 -3.169995e-17 + outer loop + vertex 3.830222e-02 3.213938e-02 -3.000000e-02 + vertex 8.682409e-03 4.924039e-02 7.000000e-02 + vertex 8.682409e-03 4.924039e-02 -3.000000e-02 + endloop + endfacet + facet normal 1.736482e-01 -9.848078e-01 2.108621e-17 + outer loop + vertex 8.682409e-03 4.924039e-02 -3.000000e-02 + vertex 8.682409e-03 4.924039e-02 7.000000e-02 + vertex -2.500000e-02 4.330127e-02 7.000000e-02 + endloop + endfacet + facet normal 1.736482e-01 -9.848078e-01 -5.547491e-17 + outer loop + vertex 8.682409e-03 4.924039e-02 -3.000000e-02 + vertex -2.500000e-02 4.330127e-02 7.000000e-02 + vertex -2.500000e-02 4.330127e-02 -3.000000e-02 + endloop + endfacet + facet normal 7.660444e-01 -6.427876e-01 8.552661e-18 + outer loop + vertex -2.500000e-02 4.330127e-02 -3.000000e-02 + vertex -2.500000e-02 4.330127e-02 7.000000e-02 + vertex -4.698463e-02 1.710101e-02 7.000000e-02 + endloop + endfacet + facet normal 7.660444e-01 -6.427876e-01 -6.339990e-17 + outer loop + vertex -2.500000e-02 4.330127e-02 -3.000000e-02 + vertex -4.698463e-02 1.710101e-02 7.000000e-02 + vertex -4.698463e-02 1.710101e-02 -3.000000e-02 + endloop + endfacet + facet normal 1.000000e+00 0.000000e+00 0.000000e+00 + outer loop + vertex -4.698463e-02 1.710101e-02 -3.000000e-02 + vertex -4.698463e-02 1.710101e-02 7.000000e-02 + vertex -4.698463e-02 -1.710101e-02 7.000000e-02 + endloop + endfacet + facet normal 1.000000e+00 0.000000e+00 0.000000e+00 + outer loop + vertex -4.698463e-02 1.710101e-02 -3.000000e-02 + vertex -4.698463e-02 -1.710101e-02 7.000000e-02 + vertex -4.698463e-02 -1.710101e-02 -3.000000e-02 + endloop + endfacet + facet normal 7.660444e-01 6.427876e-01 6.690353e-17 + outer loop + vertex -4.698463e-02 -1.710101e-02 -3.000000e-02 + vertex -4.698463e-02 -1.710101e-02 7.000000e-02 + vertex -2.500000e-02 -4.330127e-02 7.000000e-02 + endloop + endfacet + facet normal 7.660444e-01 6.427876e-01 -3.169995e-17 + outer loop + vertex -4.698463e-02 -1.710101e-02 -3.000000e-02 + vertex -2.500000e-02 -4.330127e-02 7.000000e-02 + vertex -2.500000e-02 -4.330127e-02 -3.000000e-02 + endloop + endfacet + facet normal 1.736482e-01 9.848078e-01 -6.231013e-17 + outer loop + vertex -2.500000e-02 -4.330127e-02 -3.000000e-02 + vertex -2.500000e-02 -4.330127e-02 7.000000e-02 + vertex 8.682409e-03 -4.924039e-02 7.000000e-02 + endloop + endfacet + facet normal 1.736482e-01 9.848078e-01 -1.030248e-16 + outer loop + vertex -2.500000e-02 -4.330127e-02 -3.000000e-02 + vertex 8.682409e-03 -4.924039e-02 7.000000e-02 + vertex 8.682409e-03 -4.924039e-02 -3.000000e-02 + endloop + endfacet + facet normal -5.000000e-01 8.660254e-01 4.399082e-17 + outer loop + vertex 8.682409e-03 -4.924039e-02 -3.000000e-02 + vertex 8.682409e-03 -4.924039e-02 7.000000e-02 + vertex 3.830222e-02 -3.213938e-02 7.000000e-02 + endloop + endfacet + facet normal -5.000000e-01 8.660254e-01 -3.169995e-17 + outer loop + vertex 8.682409e-03 -4.924039e-02 -3.000000e-02 + vertex 3.830222e-02 -3.213938e-02 7.000000e-02 + vertex 3.830222e-02 -3.213938e-02 -3.000000e-02 + endloop + endfacet + facet normal -9.396926e-01 3.420201e-01 8.294372e-17 + outer loop + vertex 3.830222e-02 -3.213938e-02 -3.000000e-02 + vertex 3.830222e-02 -3.213938e-02 7.000000e-02 + vertex 5.000000e-02 0.000000e+00 7.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 0.000000e+00 1.000000e+00 + outer loop + vertex 3.830222e-02 3.213938e-02 -3.000000e-02 + vertex -4.698463e-02 1.710101e-02 -3.000000e-02 + vertex 5.000000e-02 0.000000e+00 -3.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 0.000000e+00 1.000000e+00 + outer loop + vertex 5.000000e-02 0.000000e+00 -3.000000e-02 + vertex -4.698463e-02 1.710101e-02 -3.000000e-02 + vertex -4.698463e-02 -1.710101e-02 -3.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 0.000000e+00 1.000000e+00 + outer loop + vertex 5.000000e-02 0.000000e+00 -3.000000e-02 + vertex -4.698463e-02 -1.710101e-02 -3.000000e-02 + vertex 3.830222e-02 -3.213938e-02 -3.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 0.000000e+00 1.000000e+00 + outer loop + vertex 3.830222e-02 -3.213938e-02 -3.000000e-02 + vertex -4.698463e-02 -1.710101e-02 -3.000000e-02 + vertex -2.500000e-02 -4.330127e-02 -3.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 0.000000e+00 1.000000e+00 + outer loop + vertex 3.830222e-02 -3.213938e-02 -3.000000e-02 + vertex -2.500000e-02 -4.330127e-02 -3.000000e-02 + vertex 8.682409e-03 -4.924039e-02 -3.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 0.000000e+00 1.000000e+00 + outer loop + vertex 8.682409e-03 4.924039e-02 -3.000000e-02 + vertex -2.500000e-02 4.330127e-02 -3.000000e-02 + vertex 3.830222e-02 3.213938e-02 -3.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 0.000000e+00 1.000000e+00 + outer loop + vertex 3.830222e-02 3.213938e-02 -3.000000e-02 + vertex -2.500000e-02 4.330127e-02 -3.000000e-02 + vertex -4.698463e-02 1.710101e-02 -3.000000e-02 + endloop + endfacet + facet normal 9.270530e-01 -3.374197e-01 -1.634645e-01 + outer loop + vertex 8.809511e-02 -7.392058e-02 -3.000000e-02 + vertex 8.624719e-02 -7.236998e-02 -4.368081e-02 + vertex 1.150000e-01 -2.816688e-17 -3.000000e-02 + endloop + endfacet + facet normal 9.270530e-01 -3.374197e-01 -1.634645e-01 + outer loop + vertex 1.150000e-01 -2.816688e-17 -3.000000e-02 + vertex 8.624719e-02 -7.236998e-02 -4.368081e-02 + vertex 1.125877e-01 1.378802e-17 -4.368081e-02 + endloop + endfacet + facet normal 9.270530e-01 3.374197e-01 -1.634645e-01 + outer loop + vertex 1.150000e-01 -2.816688e-17 -3.000000e-02 + vertex 1.125877e-01 1.378802e-17 -4.368081e-02 + vertex 8.809511e-02 7.392058e-02 -3.000000e-02 + endloop + endfacet + facet normal 9.270530e-01 3.374197e-01 -1.634645e-01 + outer loop + vertex 8.809511e-02 7.392058e-02 -3.000000e-02 + vertex 1.125877e-01 1.378802e-17 -4.368081e-02 + vertex 8.624719e-02 7.236998e-02 -4.368081e-02 + endloop + endfacet + facet normal 4.932746e-01 8.543767e-01 -1.634645e-01 + outer loop + vertex 8.809511e-02 7.392058e-02 -3.000000e-02 + vertex 8.624719e-02 7.236998e-02 -4.368081e-02 + vertex 1.996954e-02 1.132529e-01 -3.000000e-02 + endloop + endfacet + facet normal 4.932746e-01 8.543767e-01 -1.634645e-01 + outer loop + vertex 1.996954e-02 1.132529e-01 -3.000000e-02 + vertex 8.624719e-02 7.236998e-02 -4.368081e-02 + vertex 1.955065e-02 1.108772e-01 -4.368081e-02 + endloop + endfacet + facet normal -1.713125e-01 9.715613e-01 -1.634645e-01 + outer loop + vertex 1.996954e-02 1.132529e-01 -3.000000e-02 + vertex 1.955065e-02 1.108772e-01 -4.368081e-02 + vertex -5.750000e-02 9.959292e-02 -3.000000e-02 + endloop + endfacet + facet normal -1.713125e-01 9.715613e-01 -1.634645e-01 + outer loop + vertex -5.750000e-02 9.959292e-02 -3.000000e-02 + vertex 1.955065e-02 1.108772e-01 -4.368081e-02 + vertex -5.629385e-02 9.750381e-02 -4.368081e-02 + endloop + endfacet + facet normal -7.557406e-01 6.341416e-01 -1.634645e-01 + outer loop + vertex -5.750000e-02 9.959292e-02 -3.000000e-02 + vertex -5.629385e-02 9.750381e-02 -4.368081e-02 + vertex -1.080647e-01 3.933232e-02 -3.000000e-02 + endloop + endfacet + facet normal -7.557406e-01 6.341416e-01 -1.634645e-01 + outer loop + vertex -1.080647e-01 3.933232e-02 -3.000000e-02 + vertex -5.629385e-02 9.750381e-02 -4.368081e-02 + vertex -1.057978e-01 3.850726e-02 -4.368081e-02 + endloop + endfacet + facet normal -9.865492e-01 -0.000000e+00 -1.634645e-01 + outer loop + vertex -1.080647e-01 3.933232e-02 -3.000000e-02 + vertex -1.057978e-01 3.850726e-02 -4.368081e-02 + vertex -1.080647e-01 -3.933232e-02 -3.000000e-02 + endloop + endfacet + facet normal -9.865492e-01 3.553144e-16 -1.634645e-01 + outer loop + vertex -1.080647e-01 -3.933232e-02 -3.000000e-02 + vertex -1.057978e-01 3.850726e-02 -4.368081e-02 + vertex -1.057978e-01 -3.850726e-02 -4.368081e-02 + endloop + endfacet + facet normal -7.557406e-01 -6.341416e-01 -1.634645e-01 + outer loop + vertex -1.080647e-01 -3.933232e-02 -3.000000e-02 + vertex -1.057978e-01 -3.850726e-02 -4.368081e-02 + vertex -5.750000e-02 -9.959292e-02 -3.000000e-02 + endloop + endfacet + facet normal -7.557406e-01 -6.341416e-01 -1.634645e-01 + outer loop + vertex -5.750000e-02 -9.959292e-02 -3.000000e-02 + vertex -1.057978e-01 -3.850726e-02 -4.368081e-02 + vertex -5.629385e-02 -9.750381e-02 -4.368081e-02 + endloop + endfacet + facet normal -1.713125e-01 -9.715613e-01 -1.634645e-01 + outer loop + vertex -5.750000e-02 -9.959292e-02 -3.000000e-02 + vertex -5.629385e-02 -9.750381e-02 -4.368081e-02 + vertex 1.996954e-02 -1.132529e-01 -3.000000e-02 + endloop + endfacet + facet normal -1.713125e-01 -9.715613e-01 -1.634645e-01 + outer loop + vertex 1.996954e-02 -1.132529e-01 -3.000000e-02 + vertex -5.629385e-02 -9.750381e-02 -4.368081e-02 + vertex 1.955065e-02 -1.108772e-01 -4.368081e-02 + endloop + endfacet + facet normal 4.932746e-01 -8.543767e-01 -1.634645e-01 + outer loop + vertex 1.996954e-02 -1.132529e-01 -3.000000e-02 + vertex 1.955065e-02 -1.108772e-01 -4.368081e-02 + vertex 8.809511e-02 -7.392058e-02 -3.000000e-02 + endloop + endfacet + facet normal 4.932746e-01 -8.543767e-01 -1.634645e-01 + outer loop + vertex 8.809511e-02 -7.392058e-02 -3.000000e-02 + vertex 1.955065e-02 -1.108772e-01 -4.368081e-02 + vertex 8.624719e-02 -7.236998e-02 -4.368081e-02 + endloop + endfacet + facet normal 2.576782e-01 9.378721e-02 -9.616683e-01 + outer loop + vertex 5.745333e-02 4.820907e-02 -7.000000e-02 + vertex 7.277422e-02 6.106482e-02 -6.464102e-02 + vertex 7.500000e-02 0.000000e+00 -7.000000e-02 + endloop + endfacet + facet normal 2.576782e-01 9.378721e-02 -9.616683e-01 + outer loop + vertex 7.500000e-02 0.000000e+00 -7.000000e-02 + vertex 7.277422e-02 6.106482e-02 -6.464102e-02 + vertex 9.500000e-02 1.163414e-17 -6.464102e-02 + endloop + endfacet + facet normal 2.576782e-01 -9.378721e-02 -9.616683e-01 + outer loop + vertex 7.500000e-02 0.000000e+00 -7.000000e-02 + vertex 9.500000e-02 1.163414e-17 -6.464102e-02 + vertex 5.745333e-02 -4.820907e-02 -7.000000e-02 + endloop + endfacet + facet normal 2.576782e-01 -9.378721e-02 -9.616683e-01 + outer loop + vertex 5.745333e-02 -4.820907e-02 -7.000000e-02 + vertex 9.500000e-02 1.163414e-17 -6.464102e-02 + vertex 7.277422e-02 -6.106482e-02 -6.464102e-02 + endloop + endfacet + facet normal 1.371077e-01 -2.374776e-01 -9.616683e-01 + outer loop + vertex 5.745333e-02 -4.820907e-02 -7.000000e-02 + vertex 7.277422e-02 -6.106482e-02 -6.464102e-02 + vertex 1.302361e-02 -7.386058e-02 -7.000000e-02 + endloop + endfacet + facet normal 1.371077e-01 -2.374776e-01 -9.616683e-01 + outer loop + vertex 1.302361e-02 -7.386058e-02 -7.000000e-02 + vertex 7.277422e-02 -6.106482e-02 -6.464102e-02 + vertex 1.649658e-02 -9.355674e-02 -6.464102e-02 + endloop + endfacet + facet normal -4.761701e-02 -2.700495e-01 -9.616683e-01 + outer loop + vertex 1.302361e-02 -7.386058e-02 -7.000000e-02 + vertex 1.649658e-02 -9.355674e-02 -6.464102e-02 + vertex -3.750000e-02 -6.495191e-02 -7.000000e-02 + endloop + endfacet + facet normal -4.761701e-02 -2.700495e-01 -9.616683e-01 + outer loop + vertex -3.750000e-02 -6.495191e-02 -7.000000e-02 + vertex 1.649658e-02 -9.355674e-02 -6.464102e-02 + vertex -4.750000e-02 -8.227241e-02 -6.464102e-02 + endloop + endfacet + facet normal -2.100612e-01 -1.762623e-01 -9.616683e-01 + outer loop + vertex -3.750000e-02 -6.495191e-02 -7.000000e-02 + vertex -4.750000e-02 -8.227241e-02 -6.464102e-02 + vertex -7.047695e-02 -2.565151e-02 -7.000000e-02 + endloop + endfacet + facet normal -2.100612e-01 -1.762623e-01 -9.616683e-01 + outer loop + vertex -7.047695e-02 -2.565151e-02 -7.000000e-02 + vertex -4.750000e-02 -8.227241e-02 -6.464102e-02 + vertex -8.927080e-02 -3.249191e-02 -6.464102e-02 + endloop + endfacet + facet normal -2.742155e-01 0.000000e+00 -9.616683e-01 + outer loop + vertex -7.047695e-02 -2.565151e-02 -7.000000e-02 + vertex -8.927080e-02 -3.249191e-02 -6.464102e-02 + vertex -7.047695e-02 2.565151e-02 -7.000000e-02 + endloop + endfacet + facet normal -2.742155e-01 6.402885e-17 -9.616683e-01 + outer loop + vertex -7.047695e-02 2.565151e-02 -7.000000e-02 + vertex -8.927080e-02 -3.249191e-02 -6.464102e-02 + vertex -8.927080e-02 3.249191e-02 -6.464102e-02 + endloop + endfacet + facet normal -2.100612e-01 1.762623e-01 -9.616683e-01 + outer loop + vertex -7.047695e-02 2.565151e-02 -7.000000e-02 + vertex -8.927080e-02 3.249191e-02 -6.464102e-02 + vertex -3.750000e-02 6.495191e-02 -7.000000e-02 + endloop + endfacet + facet normal -2.100612e-01 1.762623e-01 -9.616683e-01 + outer loop + vertex -3.750000e-02 6.495191e-02 -7.000000e-02 + vertex -8.927080e-02 3.249191e-02 -6.464102e-02 + vertex -4.750000e-02 8.227241e-02 -6.464102e-02 + endloop + endfacet + facet normal -4.761701e-02 2.700495e-01 -9.616683e-01 + outer loop + vertex -3.750000e-02 6.495191e-02 -7.000000e-02 + vertex -4.750000e-02 8.227241e-02 -6.464102e-02 + vertex 1.302361e-02 7.386058e-02 -7.000000e-02 + endloop + endfacet + facet normal -4.761701e-02 2.700495e-01 -9.616683e-01 + outer loop + vertex 1.302361e-02 7.386058e-02 -7.000000e-02 + vertex -4.750000e-02 8.227241e-02 -6.464102e-02 + vertex 1.649658e-02 9.355674e-02 -6.464102e-02 + endloop + endfacet + facet normal 1.371077e-01 2.374776e-01 -9.616683e-01 + outer loop + vertex 1.302361e-02 7.386058e-02 -7.000000e-02 + vertex 1.649658e-02 9.355674e-02 -6.464102e-02 + vertex 5.745333e-02 4.820907e-02 -7.000000e-02 + endloop + endfacet + facet normal 1.371077e-01 2.374776e-01 -9.616683e-01 + outer loop + vertex 5.745333e-02 4.820907e-02 -7.000000e-02 + vertex 1.649658e-02 9.355674e-02 -6.464102e-02 + vertex 7.277422e-02 6.106482e-02 -6.464102e-02 + endloop + endfacet + facet normal 7.378994e-01 2.685734e-01 -6.191711e-01 + outer loop + vertex 1.125877e-01 1.378802e-17 -4.368081e-02 + vertex 9.500000e-02 1.163414e-17 -6.464102e-02 + vertex 8.624719e-02 7.236998e-02 -4.368081e-02 + endloop + endfacet + facet normal 7.378994e-01 2.685734e-01 -6.191711e-01 + outer loop + vertex 8.624719e-02 7.236998e-02 -4.368081e-02 + vertex 9.500000e-02 1.163414e-17 -6.464102e-02 + vertex 7.277422e-02 6.106482e-02 -6.464102e-02 + endloop + endfacet + facet normal 3.926281e-01 6.800517e-01 -6.191711e-01 + outer loop + vertex 8.624719e-02 7.236998e-02 -4.368081e-02 + vertex 7.277422e-02 6.106482e-02 -6.464102e-02 + vertex 1.955065e-02 1.108772e-01 -4.368081e-02 + endloop + endfacet + facet normal 3.926281e-01 6.800517e-01 -6.191711e-01 + outer loop + vertex 1.955065e-02 1.108772e-01 -4.368081e-02 + vertex 7.277422e-02 6.106482e-02 -6.464102e-02 + vertex 1.649658e-02 9.355674e-02 -6.464102e-02 + endloop + endfacet + facet normal -1.363583e-01 7.733263e-01 -6.191711e-01 + outer loop + vertex 1.955065e-02 1.108772e-01 -4.368081e-02 + vertex 1.649658e-02 9.355674e-02 -6.464102e-02 + vertex -5.629385e-02 9.750381e-02 -4.368081e-02 + endloop + endfacet + facet normal -1.363583e-01 7.733263e-01 -6.191711e-01 + outer loop + vertex -5.629385e-02 9.750381e-02 -4.368081e-02 + vertex 1.649658e-02 9.355674e-02 -6.464102e-02 + vertex -4.750000e-02 8.227241e-02 -6.464102e-02 + endloop + endfacet + facet normal -6.015411e-01 5.047529e-01 -6.191711e-01 + outer loop + vertex -5.629385e-02 9.750381e-02 -4.368081e-02 + vertex -4.750000e-02 8.227241e-02 -6.464102e-02 + vertex -1.057978e-01 3.850726e-02 -4.368081e-02 + endloop + endfacet + facet normal -6.015411e-01 5.047529e-01 -6.191711e-01 + outer loop + vertex -1.057978e-01 3.850726e-02 -4.368081e-02 + vertex -4.750000e-02 8.227241e-02 -6.464102e-02 + vertex -8.927080e-02 3.249191e-02 -6.464102e-02 + endloop + endfacet + facet normal -7.852561e-01 2.830016e-16 -6.191711e-01 + outer loop + vertex -1.057978e-01 3.850726e-02 -4.368081e-02 + vertex -8.927080e-02 3.249191e-02 -6.464102e-02 + vertex -1.057978e-01 -3.850726e-02 -4.368081e-02 + endloop + endfacet + facet normal -7.852561e-01 1.562646e-16 -6.191711e-01 + outer loop + vertex -1.057978e-01 -3.850726e-02 -4.368081e-02 + vertex -8.927080e-02 3.249191e-02 -6.464102e-02 + vertex -8.927080e-02 -3.249191e-02 -6.464102e-02 + endloop + endfacet + facet normal -6.015411e-01 -5.047529e-01 -6.191711e-01 + outer loop + vertex -1.057978e-01 -3.850726e-02 -4.368081e-02 + vertex -8.927080e-02 -3.249191e-02 -6.464102e-02 + vertex -5.629385e-02 -9.750381e-02 -4.368081e-02 + endloop + endfacet + facet normal -6.015411e-01 -5.047529e-01 -6.191711e-01 + outer loop + vertex -5.629385e-02 -9.750381e-02 -4.368081e-02 + vertex -8.927080e-02 -3.249191e-02 -6.464102e-02 + vertex -4.750000e-02 -8.227241e-02 -6.464102e-02 + endloop + endfacet + facet normal -1.363583e-01 -7.733263e-01 -6.191711e-01 + outer loop + vertex -5.629385e-02 -9.750381e-02 -4.368081e-02 + vertex -4.750000e-02 -8.227241e-02 -6.464102e-02 + vertex 1.955065e-02 -1.108772e-01 -4.368081e-02 + endloop + endfacet + facet normal -1.363583e-01 -7.733263e-01 -6.191711e-01 + outer loop + vertex 1.955065e-02 -1.108772e-01 -4.368081e-02 + vertex -4.750000e-02 -8.227241e-02 -6.464102e-02 + vertex 1.649658e-02 -9.355674e-02 -6.464102e-02 + endloop + endfacet + facet normal 3.926281e-01 -6.800517e-01 -6.191711e-01 + outer loop + vertex 1.955065e-02 -1.108772e-01 -4.368081e-02 + vertex 1.649658e-02 -9.355674e-02 -6.464102e-02 + vertex 8.624719e-02 -7.236998e-02 -4.368081e-02 + endloop + endfacet + facet normal 3.926281e-01 -6.800517e-01 -6.191711e-01 + outer loop + vertex 8.624719e-02 -7.236998e-02 -4.368081e-02 + vertex 1.649658e-02 -9.355674e-02 -6.464102e-02 + vertex 7.277422e-02 -6.106482e-02 -6.464102e-02 + endloop + endfacet + facet normal 7.378994e-01 -2.685734e-01 -6.191711e-01 + outer loop + vertex 8.624719e-02 -7.236998e-02 -4.368081e-02 + vertex 7.277422e-02 -6.106482e-02 -6.464102e-02 + vertex 1.125877e-01 1.378802e-17 -4.368081e-02 + endloop + endfacet + facet normal 7.378994e-01 -2.685734e-01 -6.191711e-01 + outer loop + vertex 1.125877e-01 1.378802e-17 -4.368081e-02 + vertex 7.277422e-02 -6.106482e-02 -6.464102e-02 + vertex 9.500000e-02 1.163414e-17 -6.464102e-02 + endloop + endfacet + facet normal 9.396926e-01 -3.420201e-01 1.837678e-16 + outer loop + vertex 8.809511e-02 -7.392058e-02 3.000000e-02 + vertex 1.150000e-01 -2.816688e-17 -3.000000e-02 + vertex 1.150000e-01 0.000000e+00 3.000000e-02 + endloop + endfacet + facet normal 9.396926e-01 3.420201e-01 -1.605607e-16 + outer loop + vertex 1.150000e-01 0.000000e+00 3.000000e-02 + vertex 1.150000e-01 -2.816688e-17 -3.000000e-02 + vertex 8.809511e-02 7.392058e-02 -3.000000e-02 + endloop + endfacet + facet normal 9.396926e-01 3.420201e-01 0.000000e+00 + outer loop + vertex 1.150000e-01 0.000000e+00 3.000000e-02 + vertex 8.809511e-02 7.392058e-02 -3.000000e-02 + vertex 8.809511e-02 7.392058e-02 3.000000e-02 + endloop + endfacet + facet normal 5.000000e-01 8.660254e-01 0.000000e+00 + outer loop + vertex 8.809511e-02 7.392058e-02 3.000000e-02 + vertex 8.809511e-02 7.392058e-02 -3.000000e-02 + vertex 1.996954e-02 1.132529e-01 -3.000000e-02 + endloop + endfacet + facet normal 5.000000e-01 8.660254e-01 -2.756517e-16 + outer loop + vertex 8.809511e-02 7.392058e-02 3.000000e-02 + vertex 1.996954e-02 1.132529e-01 -3.000000e-02 + vertex 1.996954e-02 1.132529e-01 3.000000e-02 + endloop + endfacet + facet normal -1.736482e-01 9.848078e-01 8.032842e-17 + outer loop + vertex 1.996954e-02 1.132529e-01 3.000000e-02 + vertex 1.996954e-02 1.132529e-01 -3.000000e-02 + vertex -5.750000e-02 9.959292e-02 -3.000000e-02 + endloop + endfacet + facet normal -1.736482e-01 9.848078e-01 -5.972455e-16 + outer loop + vertex 1.996954e-02 1.132529e-01 3.000000e-02 + vertex -5.750000e-02 9.959292e-02 -3.000000e-02 + vertex -5.750000e-02 9.959292e-02 3.000000e-02 + endloop + endfacet + facet normal -7.660444e-01 6.427876e-01 8.552661e-17 + outer loop + vertex -5.750000e-02 9.959292e-02 3.000000e-02 + vertex -5.750000e-02 9.959292e-02 -3.000000e-02 + vertex -1.080647e-01 3.933232e-02 -3.000000e-02 + endloop + endfacet + facet normal -7.660444e-01 6.427876e-01 0.000000e+00 + outer loop + vertex -5.750000e-02 9.959292e-02 3.000000e-02 + vertex -1.080647e-01 3.933232e-02 -3.000000e-02 + vertex -1.080647e-01 3.933232e-02 3.000000e-02 + endloop + endfacet + facet normal -1.000000e+00 0.000000e+00 -0.000000e+00 + outer loop + vertex -1.080647e-01 3.933232e-02 3.000000e-02 + vertex -1.080647e-01 3.933232e-02 -3.000000e-02 + vertex -1.080647e-01 -3.933232e-02 -3.000000e-02 + endloop + endfacet + facet normal -1.000000e+00 -0.000000e+00 0.000000e+00 + outer loop + vertex -1.080647e-01 3.933232e-02 3.000000e-02 + vertex -1.080647e-01 -3.933232e-02 -3.000000e-02 + vertex -1.080647e-01 -3.933232e-02 3.000000e-02 + endloop + endfacet + facet normal -7.660444e-01 -6.427876e-01 -0.000000e+00 + outer loop + vertex -1.080647e-01 -3.933232e-02 3.000000e-02 + vertex -1.080647e-01 -3.933232e-02 -3.000000e-02 + vertex -5.750000e-02 -9.959292e-02 -3.000000e-02 + endloop + endfacet + facet normal -7.660444e-01 -6.427876e-01 -9.188392e-17 + outer loop + vertex -1.080647e-01 -3.933232e-02 3.000000e-02 + vertex -5.750000e-02 -9.959292e-02 -3.000000e-02 + vertex -5.750000e-02 -9.959292e-02 3.000000e-02 + endloop + endfacet + facet normal -1.736482e-01 -9.848078e-01 5.628550e-16 + outer loop + vertex -5.750000e-02 -9.959292e-02 3.000000e-02 + vertex -5.750000e-02 -9.959292e-02 -3.000000e-02 + vertex 1.996954e-02 -1.132529e-01 -3.000000e-02 + endloop + endfacet + facet normal -1.736482e-01 -9.848078e-01 -4.594196e-17 + outer loop + vertex -5.750000e-02 -9.959292e-02 3.000000e-02 + vertex 1.996954e-02 -1.132529e-01 -3.000000e-02 + vertex 1.996954e-02 -1.132529e-01 3.000000e-02 + endloop + endfacet + facet normal 5.000000e-01 -8.660254e-01 2.023844e-16 + outer loop + vertex 1.996954e-02 -1.132529e-01 3.000000e-02 + vertex 1.996954e-02 -1.132529e-01 -3.000000e-02 + vertex 8.809511e-02 -7.392058e-02 -3.000000e-02 + endloop + endfacet + facet normal 5.000000e-01 -8.660254e-01 0.000000e+00 + outer loop + vertex 1.996954e-02 -1.132529e-01 3.000000e-02 + vertex 8.809511e-02 -7.392058e-02 -3.000000e-02 + vertex 8.809511e-02 -7.392058e-02 3.000000e-02 + endloop + endfacet + facet normal 9.396926e-01 -3.420201e-01 0.000000e+00 + outer loop + vertex 8.809511e-02 -7.392058e-02 3.000000e-02 + vertex 8.809511e-02 -7.392058e-02 -3.000000e-02 + vertex 1.150000e-01 -2.816688e-17 -3.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 0.000000e+00 -1.000000e+00 + outer loop + vertex 5.745333e-02 -4.820907e-02 -7.000000e-02 + vertex -7.047695e-02 -2.565151e-02 -7.000000e-02 + vertex 7.500000e-02 0.000000e+00 -7.000000e-02 + endloop + endfacet + facet normal -0.000000e+00 0.000000e+00 -1.000000e+00 + outer loop + vertex 7.500000e-02 0.000000e+00 -7.000000e-02 + vertex -7.047695e-02 -2.565151e-02 -7.000000e-02 + vertex -7.047695e-02 2.565151e-02 -7.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 0.000000e+00 -1.000000e+00 + outer loop + vertex 7.500000e-02 0.000000e+00 -7.000000e-02 + vertex -7.047695e-02 2.565151e-02 -7.000000e-02 + vertex 5.745333e-02 4.820907e-02 -7.000000e-02 + endloop + endfacet + facet normal -0.000000e+00 0.000000e+00 -1.000000e+00 + outer loop + vertex 5.745333e-02 4.820907e-02 -7.000000e-02 + vertex -7.047695e-02 2.565151e-02 -7.000000e-02 + vertex -3.750000e-02 6.495191e-02 -7.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 0.000000e+00 -1.000000e+00 + outer loop + vertex 5.745333e-02 4.820907e-02 -7.000000e-02 + vertex -3.750000e-02 6.495191e-02 -7.000000e-02 + vertex 1.302361e-02 7.386058e-02 -7.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 0.000000e+00 -1.000000e+00 + outer loop + vertex 1.302361e-02 -7.386058e-02 -7.000000e-02 + vertex -3.750000e-02 -6.495191e-02 -7.000000e-02 + vertex 5.745333e-02 -4.820907e-02 -7.000000e-02 + endloop + endfacet + facet normal -0.000000e+00 0.000000e+00 -1.000000e+00 + outer loop + vertex 5.745333e-02 -4.820907e-02 -7.000000e-02 + vertex -3.750000e-02 -6.495191e-02 -7.000000e-02 + vertex -7.047695e-02 -2.565151e-02 -7.000000e-02 + endloop + endfacet + facet normal 8.921961e-01 3.247328e-01 -3.139023e-01 + outer loop + vertex 8.809511e-02 7.392058e-02 3.000000e-02 + vertex 1.057978e-01 3.850726e-02 4.368081e-02 + vertex 1.150000e-01 0.000000e+00 3.000000e-02 + endloop + endfacet + facet normal 8.297582e-01 0.000000e+00 5.581230e-01 + outer loop + vertex 1.150000e-01 0.000000e+00 3.000000e-02 + vertex 1.057978e-01 3.850726e-02 4.368081e-02 + vertex 1.057978e-01 -3.850726e-02 4.368081e-02 + endloop + endfacet + facet normal 8.921961e-01 -3.247328e-01 -3.139023e-01 + outer loop + vertex 1.150000e-01 0.000000e+00 3.000000e-02 + vertex 1.057978e-01 -3.850726e-02 4.368081e-02 + vertex 8.809511e-02 -7.392058e-02 3.000000e-02 + endloop + endfacet + facet normal 6.356317e-01 -5.333583e-01 5.581230e-01 + outer loop + vertex 8.809511e-02 -7.392058e-02 3.000000e-02 + vertex 1.057978e-01 -3.850726e-02 4.368081e-02 + vertex 5.629385e-02 -9.750381e-02 4.368081e-02 + endloop + endfacet + facet normal 4.747276e-01 -8.222524e-01 -3.139023e-01 + outer loop + vertex 8.809511e-02 -7.392058e-02 3.000000e-02 + vertex 5.629385e-02 -9.750381e-02 4.368081e-02 + vertex 1.996954e-02 -1.132529e-01 3.000000e-02 + endloop + endfacet + facet normal 1.440860e-01 -8.171524e-01 5.581230e-01 + outer loop + vertex 1.996954e-02 -1.132529e-01 3.000000e-02 + vertex 5.629385e-02 -9.750381e-02 4.368081e-02 + vertex -1.955065e-02 -1.108772e-01 4.368081e-02 + endloop + endfacet + facet normal -1.648712e-01 -9.350309e-01 -3.139023e-01 + outer loop + vertex 1.996954e-02 -1.132529e-01 3.000000e-02 + vertex -1.955065e-02 -1.108772e-01 4.368081e-02 + vertex -5.750000e-02 -9.959292e-02 3.000000e-02 + endloop + endfacet + facet normal -4.148791e-01 -7.185917e-01 5.581230e-01 + outer loop + vertex -5.750000e-02 -9.959292e-02 3.000000e-02 + vertex -1.955065e-02 -1.108772e-01 4.368081e-02 + vertex -8.624719e-02 -7.236998e-02 4.368081e-02 + endloop + endfacet + facet normal -7.273249e-01 -6.102981e-01 -3.139023e-01 + outer loop + vertex -5.750000e-02 -9.959292e-02 3.000000e-02 + vertex -8.624719e-02 -7.236998e-02 4.368081e-02 + vertex -1.080647e-01 -3.933232e-02 3.000000e-02 + endloop + endfacet + facet normal -7.797177e-01 -2.837940e-01 5.581230e-01 + outer loop + vertex -1.080647e-01 -3.933232e-02 3.000000e-02 + vertex -8.624719e-02 -7.236998e-02 4.368081e-02 + vertex -1.125877e-01 1.378802e-17 4.368081e-02 + endloop + endfacet + facet normal -9.494553e-01 0.000000e+00 -3.139023e-01 + outer loop + vertex -1.080647e-01 -3.933232e-02 3.000000e-02 + vertex -1.125877e-01 1.378802e-17 4.368081e-02 + vertex -1.080647e-01 3.933232e-02 3.000000e-02 + endloop + endfacet + facet normal -7.797177e-01 2.837940e-01 5.581230e-01 + outer loop + vertex -1.080647e-01 3.933232e-02 3.000000e-02 + vertex -1.125877e-01 1.378802e-17 4.368081e-02 + vertex -8.624719e-02 7.236998e-02 4.368081e-02 + endloop + endfacet + facet normal -7.273249e-01 6.102981e-01 -3.139023e-01 + outer loop + vertex -1.080647e-01 3.933232e-02 3.000000e-02 + vertex -8.624719e-02 7.236998e-02 4.368081e-02 + vertex -5.750000e-02 9.959292e-02 3.000000e-02 + endloop + endfacet + facet normal -4.148791e-01 7.185917e-01 5.581230e-01 + outer loop + vertex -5.750000e-02 9.959292e-02 3.000000e-02 + vertex -8.624719e-02 7.236998e-02 4.368081e-02 + vertex -1.955065e-02 1.108772e-01 4.368081e-02 + endloop + endfacet + facet normal -1.648712e-01 9.350309e-01 -3.139023e-01 + outer loop + vertex -5.750000e-02 9.959292e-02 3.000000e-02 + vertex -1.955065e-02 1.108772e-01 4.368081e-02 + vertex 1.996954e-02 1.132529e-01 3.000000e-02 + endloop + endfacet + facet normal 1.440860e-01 8.171524e-01 5.581230e-01 + outer loop + vertex 1.996954e-02 1.132529e-01 3.000000e-02 + vertex -1.955065e-02 1.108772e-01 4.368081e-02 + vertex 5.629385e-02 9.750381e-02 4.368081e-02 + endloop + endfacet + facet normal 4.747276e-01 8.222524e-01 -3.139023e-01 + outer loop + vertex 1.996954e-02 1.132529e-01 3.000000e-02 + vertex 5.629385e-02 9.750381e-02 4.368081e-02 + vertex 8.809511e-02 7.392058e-02 3.000000e-02 + endloop + endfacet + facet normal 6.356317e-01 5.333583e-01 5.581230e-01 + outer loop + vertex 8.809511e-02 7.392058e-02 3.000000e-02 + vertex 5.629385e-02 9.750381e-02 4.368081e-02 + vertex 1.057978e-01 3.850726e-02 4.368081e-02 + endloop + endfacet + facet normal 2.006152e-01 -7.301798e-02 9.769452e-01 + outer loop + vertex 5.745333e-02 -4.820907e-02 7.000000e-02 + vertex 8.927080e-02 -3.249191e-02 6.464102e-02 + vertex 7.500000e-02 -1.836970e-17 7.000000e-02 + endloop + endfacet + facet normal 3.515510e-01 0.000000e+00 9.361687e-01 + outer loop + vertex 7.500000e-02 -1.836970e-17 7.000000e-02 + vertex 8.927080e-02 -3.249191e-02 6.464102e-02 + vertex 8.927080e-02 3.249191e-02 6.464102e-02 + endloop + endfacet + facet normal 2.006152e-01 7.301798e-02 9.769452e-01 + outer loop + vertex 7.500000e-02 -1.836970e-17 7.000000e-02 + vertex 8.927080e-02 3.249191e-02 6.464102e-02 + vertex 5.745333e-02 4.820907e-02 7.000000e-02 + endloop + endfacet + facet normal 2.693037e-01 2.259726e-01 9.361687e-01 + outer loop + vertex 5.745333e-02 4.820907e-02 7.000000e-02 + vertex 8.927080e-02 3.249191e-02 6.464102e-02 + vertex 4.750000e-02 8.227241e-02 6.464102e-02 + endloop + endfacet + facet normal 1.067451e-01 1.848880e-01 9.769452e-01 + outer loop + vertex 5.745333e-02 4.820907e-02 7.000000e-02 + vertex 4.750000e-02 8.227241e-02 6.464102e-02 + vertex 1.302361e-02 7.386058e-02 7.000000e-02 + endloop + endfacet + facet normal 6.104618e-02 3.462101e-01 9.361687e-01 + outer loop + vertex 1.302361e-02 7.386058e-02 7.000000e-02 + vertex 4.750000e-02 8.227241e-02 6.464102e-02 + vertex -1.649658e-02 9.355674e-02 6.464102e-02 + endloop + endfacet + facet normal -3.707220e-02 2.102469e-01 9.769452e-01 + outer loop + vertex 1.302361e-02 7.386058e-02 7.000000e-02 + vertex -1.649658e-02 9.355674e-02 6.464102e-02 + vertex -3.750000e-02 6.495191e-02 7.000000e-02 + endloop + endfacet + facet normal -1.757755e-01 3.044521e-01 9.361687e-01 + outer loop + vertex -3.750000e-02 6.495191e-02 7.000000e-02 + vertex -1.649658e-02 9.355674e-02 6.464102e-02 + vertex -7.277422e-02 6.106482e-02 6.464102e-02 + endloop + endfacet + facet normal -1.635430e-01 1.372289e-01 9.769452e-01 + outer loop + vertex -3.750000e-02 6.495191e-02 7.000000e-02 + vertex -7.277422e-02 6.106482e-02 6.464102e-02 + vertex -7.047695e-02 2.565151e-02 7.000000e-02 + endloop + endfacet + facet normal -3.303498e-01 1.202375e-01 9.361687e-01 + outer loop + vertex -7.047695e-02 2.565151e-02 7.000000e-02 + vertex -7.277422e-02 6.106482e-02 6.464102e-02 + vertex -9.500000e-02 -1.163414e-17 6.464102e-02 + endloop + endfacet + facet normal -2.134903e-01 0.000000e+00 9.769452e-01 + outer loop + vertex -7.047695e-02 2.565151e-02 7.000000e-02 + vertex -9.500000e-02 -1.163414e-17 6.464102e-02 + vertex -7.047695e-02 -2.565151e-02 7.000000e-02 + endloop + endfacet + facet normal -3.303498e-01 -1.202375e-01 9.361687e-01 + outer loop + vertex -7.047695e-02 -2.565151e-02 7.000000e-02 + vertex -9.500000e-02 -1.163414e-17 6.464102e-02 + vertex -7.277422e-02 -6.106482e-02 6.464102e-02 + endloop + endfacet + facet normal -1.635430e-01 -1.372289e-01 9.769452e-01 + outer loop + vertex -7.047695e-02 -2.565151e-02 7.000000e-02 + vertex -7.277422e-02 -6.106482e-02 6.464102e-02 + vertex -3.750000e-02 -6.495191e-02 7.000000e-02 + endloop + endfacet + facet normal -1.757755e-01 -3.044521e-01 9.361687e-01 + outer loop + vertex -3.750000e-02 -6.495191e-02 7.000000e-02 + vertex -7.277422e-02 -6.106482e-02 6.464102e-02 + vertex -1.649658e-02 -9.355674e-02 6.464102e-02 + endloop + endfacet + facet normal -3.707220e-02 -2.102469e-01 9.769452e-01 + outer loop + vertex -3.750000e-02 -6.495191e-02 7.000000e-02 + vertex -1.649658e-02 -9.355674e-02 6.464102e-02 + vertex 1.302361e-02 -7.386058e-02 7.000000e-02 + endloop + endfacet + facet normal 6.104618e-02 -3.462101e-01 9.361687e-01 + outer loop + vertex 1.302361e-02 -7.386058e-02 7.000000e-02 + vertex -1.649658e-02 -9.355674e-02 6.464102e-02 + vertex 4.750000e-02 -8.227241e-02 6.464102e-02 + endloop + endfacet + facet normal 1.067451e-01 -1.848880e-01 9.769452e-01 + outer loop + vertex 1.302361e-02 -7.386058e-02 7.000000e-02 + vertex 4.750000e-02 -8.227241e-02 6.464102e-02 + vertex 5.745333e-02 -4.820907e-02 7.000000e-02 + endloop + endfacet + facet normal 2.693037e-01 -2.259726e-01 9.361687e-01 + outer loop + vertex 5.745333e-02 -4.820907e-02 7.000000e-02 + vertex 4.750000e-02 -8.227241e-02 6.464102e-02 + vertex 8.927080e-02 -3.249191e-02 6.464102e-02 + endloop + endfacet + facet normal -7.378994e-01 2.685734e-01 6.191711e-01 + outer loop + vertex -1.125877e-01 1.378802e-17 4.368081e-02 + vertex -9.500000e-02 -1.163414e-17 6.464102e-02 + vertex -8.624719e-02 7.236998e-02 4.368081e-02 + endloop + endfacet + facet normal -7.378994e-01 2.685734e-01 6.191711e-01 + outer loop + vertex -8.624719e-02 7.236998e-02 4.368081e-02 + vertex -9.500000e-02 -1.163414e-17 6.464102e-02 + vertex -7.277422e-02 6.106482e-02 6.464102e-02 + endloop + endfacet + facet normal -3.926281e-01 6.800517e-01 6.191711e-01 + outer loop + vertex -8.624719e-02 7.236998e-02 4.368081e-02 + vertex -7.277422e-02 6.106482e-02 6.464102e-02 + vertex -1.955065e-02 1.108772e-01 4.368081e-02 + endloop + endfacet + facet normal -3.926281e-01 6.800517e-01 6.191711e-01 + outer loop + vertex -1.955065e-02 1.108772e-01 4.368081e-02 + vertex -7.277422e-02 6.106482e-02 6.464102e-02 + vertex -1.649658e-02 9.355674e-02 6.464102e-02 + endloop + endfacet + facet normal 1.363583e-01 7.733263e-01 6.191711e-01 + outer loop + vertex -1.955065e-02 1.108772e-01 4.368081e-02 + vertex -1.649658e-02 9.355674e-02 6.464102e-02 + vertex 5.629385e-02 9.750381e-02 4.368081e-02 + endloop + endfacet + facet normal 1.363583e-01 7.733263e-01 6.191711e-01 + outer loop + vertex 5.629385e-02 9.750381e-02 4.368081e-02 + vertex -1.649658e-02 9.355674e-02 6.464102e-02 + vertex 4.750000e-02 8.227241e-02 6.464102e-02 + endloop + endfacet + facet normal 6.015411e-01 5.047529e-01 6.191711e-01 + outer loop + vertex 5.629385e-02 9.750381e-02 4.368081e-02 + vertex 4.750000e-02 8.227241e-02 6.464102e-02 + vertex 1.057978e-01 3.850726e-02 4.368081e-02 + endloop + endfacet + facet normal 6.015411e-01 5.047529e-01 6.191711e-01 + outer loop + vertex 1.057978e-01 3.850726e-02 4.368081e-02 + vertex 4.750000e-02 8.227241e-02 6.464102e-02 + vertex 8.927080e-02 3.249191e-02 6.464102e-02 + endloop + endfacet + facet normal 7.852561e-01 0.000000e+00 6.191711e-01 + outer loop + vertex 1.057978e-01 3.850726e-02 4.368081e-02 + vertex 8.927080e-02 3.249191e-02 6.464102e-02 + vertex 1.057978e-01 -3.850726e-02 4.368081e-02 + endloop + endfacet + facet normal 7.852561e-01 0.000000e+00 6.191711e-01 + outer loop + vertex 1.057978e-01 -3.850726e-02 4.368081e-02 + vertex 8.927080e-02 3.249191e-02 6.464102e-02 + vertex 8.927080e-02 -3.249191e-02 6.464102e-02 + endloop + endfacet + facet normal 6.015411e-01 -5.047529e-01 6.191711e-01 + outer loop + vertex 1.057978e-01 -3.850726e-02 4.368081e-02 + vertex 8.927080e-02 -3.249191e-02 6.464102e-02 + vertex 5.629385e-02 -9.750381e-02 4.368081e-02 + endloop + endfacet + facet normal 6.015411e-01 -5.047529e-01 6.191711e-01 + outer loop + vertex 5.629385e-02 -9.750381e-02 4.368081e-02 + vertex 8.927080e-02 -3.249191e-02 6.464102e-02 + vertex 4.750000e-02 -8.227241e-02 6.464102e-02 + endloop + endfacet + facet normal 1.363583e-01 -7.733263e-01 6.191711e-01 + outer loop + vertex 5.629385e-02 -9.750381e-02 4.368081e-02 + vertex 4.750000e-02 -8.227241e-02 6.464102e-02 + vertex -1.955065e-02 -1.108772e-01 4.368081e-02 + endloop + endfacet + facet normal 1.363583e-01 -7.733263e-01 6.191711e-01 + outer loop + vertex -1.955065e-02 -1.108772e-01 4.368081e-02 + vertex 4.750000e-02 -8.227241e-02 6.464102e-02 + vertex -1.649658e-02 -9.355674e-02 6.464102e-02 + endloop + endfacet + facet normal -3.926281e-01 -6.800517e-01 6.191711e-01 + outer loop + vertex -1.955065e-02 -1.108772e-01 4.368081e-02 + vertex -1.649658e-02 -9.355674e-02 6.464102e-02 + vertex -8.624719e-02 -7.236998e-02 4.368081e-02 + endloop + endfacet + facet normal -3.926281e-01 -6.800517e-01 6.191711e-01 + outer loop + vertex -8.624719e-02 -7.236998e-02 4.368081e-02 + vertex -1.649658e-02 -9.355674e-02 6.464102e-02 + vertex -7.277422e-02 -6.106482e-02 6.464102e-02 + endloop + endfacet + facet normal -7.378994e-01 -2.685734e-01 6.191711e-01 + outer loop + vertex -8.624719e-02 -7.236998e-02 4.368081e-02 + vertex -7.277422e-02 -6.106482e-02 6.464102e-02 + vertex -1.125877e-01 1.378802e-17 4.368081e-02 + endloop + endfacet + facet normal -7.378994e-01 -2.685734e-01 6.191711e-01 + outer loop + vertex -1.125877e-01 1.378802e-17 4.368081e-02 + vertex -7.277422e-02 -6.106482e-02 6.464102e-02 + vertex -9.500000e-02 -1.163414e-17 6.464102e-02 + endloop + endfacet + facet normal -0.000000e+00 0.000000e+00 1.000000e+00 + outer loop + vertex 3.830222e-02 -3.213938e-02 7.000000e-02 + vertex 5.745333e-02 -4.820907e-02 7.000000e-02 + vertex 5.000000e-02 0.000000e+00 7.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 0.000000e+00 1.000000e+00 + outer loop + vertex 5.000000e-02 0.000000e+00 7.000000e-02 + vertex 5.745333e-02 -4.820907e-02 7.000000e-02 + vertex 7.500000e-02 -1.836970e-17 7.000000e-02 + endloop + endfacet + facet normal -0.000000e+00 -0.000000e+00 1.000000e+00 + outer loop + vertex 5.000000e-02 0.000000e+00 7.000000e-02 + vertex 7.500000e-02 -1.836970e-17 7.000000e-02 + vertex 3.830222e-02 3.213938e-02 7.000000e-02 + endloop + endfacet + facet normal -0.000000e+00 0.000000e+00 1.000000e+00 + outer loop + vertex 3.830222e-02 3.213938e-02 7.000000e-02 + vertex 7.500000e-02 -1.836970e-17 7.000000e-02 + vertex 5.745333e-02 4.820907e-02 7.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 -0.000000e+00 1.000000e+00 + outer loop + vertex 3.830222e-02 3.213938e-02 7.000000e-02 + vertex 5.745333e-02 4.820907e-02 7.000000e-02 + vertex 8.682409e-03 4.924039e-02 7.000000e-02 + endloop + endfacet + facet normal -0.000000e+00 0.000000e+00 1.000000e+00 + outer loop + vertex 8.682409e-03 4.924039e-02 7.000000e-02 + vertex 5.745333e-02 4.820907e-02 7.000000e-02 + vertex 1.302361e-02 7.386058e-02 7.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 -0.000000e+00 1.000000e+00 + outer loop + vertex 8.682409e-03 4.924039e-02 7.000000e-02 + vertex 1.302361e-02 7.386058e-02 7.000000e-02 + vertex -3.750000e-02 6.495191e-02 7.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 0.000000e+00 1.000000e+00 + outer loop + vertex 3.830222e-02 -3.213938e-02 7.000000e-02 + vertex 8.682409e-03 -4.924039e-02 7.000000e-02 + vertex 5.745333e-02 -4.820907e-02 7.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 0.000000e+00 1.000000e+00 + outer loop + vertex 5.745333e-02 -4.820907e-02 7.000000e-02 + vertex 8.682409e-03 -4.924039e-02 7.000000e-02 + vertex 1.302361e-02 -7.386058e-02 7.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 0.000000e+00 1.000000e+00 + outer loop + vertex 1.302361e-02 -7.386058e-02 7.000000e-02 + vertex 8.682409e-03 -4.924039e-02 7.000000e-02 + vertex -3.750000e-02 -6.495191e-02 7.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 0.000000e+00 1.000000e+00 + outer loop + vertex -3.750000e-02 -6.495191e-02 7.000000e-02 + vertex 8.682409e-03 -4.924039e-02 7.000000e-02 + vertex -2.500000e-02 -4.330127e-02 7.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 -0.000000e+00 1.000000e+00 + outer loop + vertex -3.750000e-02 -6.495191e-02 7.000000e-02 + vertex -2.500000e-02 -4.330127e-02 7.000000e-02 + vertex -7.047695e-02 -2.565151e-02 7.000000e-02 + endloop + endfacet + facet normal -0.000000e+00 0.000000e+00 1.000000e+00 + outer loop + vertex -7.047695e-02 -2.565151e-02 7.000000e-02 + vertex -2.500000e-02 -4.330127e-02 7.000000e-02 + vertex -4.698463e-02 -1.710101e-02 7.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 0.000000e+00 1.000000e+00 + outer loop + vertex -7.047695e-02 -2.565151e-02 7.000000e-02 + vertex -4.698463e-02 -1.710101e-02 7.000000e-02 + vertex -4.698463e-02 1.710101e-02 7.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 0.000000e+00 1.000000e+00 + outer loop + vertex -7.047695e-02 -2.565151e-02 7.000000e-02 + vertex -4.698463e-02 1.710101e-02 7.000000e-02 + vertex -7.047695e-02 2.565151e-02 7.000000e-02 + endloop + endfacet + facet normal -0.000000e+00 0.000000e+00 1.000000e+00 + outer loop + vertex -7.047695e-02 2.565151e-02 7.000000e-02 + vertex -4.698463e-02 1.710101e-02 7.000000e-02 + vertex -2.500000e-02 4.330127e-02 7.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 0.000000e+00 1.000000e+00 + outer loop + vertex -7.047695e-02 2.565151e-02 7.000000e-02 + vertex -2.500000e-02 4.330127e-02 7.000000e-02 + vertex -3.750000e-02 6.495191e-02 7.000000e-02 + endloop + endfacet + facet normal 0.000000e+00 0.000000e+00 1.000000e+00 + outer loop + vertex -3.750000e-02 6.495191e-02 7.000000e-02 + vertex -2.500000e-02 4.330127e-02 7.000000e-02 + vertex 8.682409e-03 4.924039e-02 7.000000e-02 + endloop + endfacet +endsolid diff --git a/mobile_trunk_sim/meshes/model_colision.stl b/mobile_trunk_sim/meshes/model_colision.stl new file mode 100644 index 0000000..cd37243 Binary files /dev/null and b/mobile_trunk_sim/meshes/model_colision.stl differ diff --git a/mobile_trunk_sim/meshes/trunk.stl b/mobile_trunk_sim/meshes/trunk.stl new file mode 100644 index 0000000..c05bbc1 Binary files /dev/null and b/mobile_trunk_sim/meshes/trunk.stl differ diff --git a/mobile_trunk_sim/mobile_trunk.py b/mobile_trunk_sim/mobile_trunk.py new file mode 100644 index 0000000..2363765 --- /dev/null +++ b/mobile_trunk_sim/mobile_trunk.py @@ -0,0 +1,45 @@ +from summit_xl import SummitXL +from echelon3.parameters import * +from echelon3.createEchelon import * +from addCamera import addCamera + +def mobileTrunk(modellingNode): + + ######################################### + # create summit + ######################################### + self = SummitXL(modellingNode) + + ######################################## + # createEchelon + ######################################## + + AttachedArm = self.Chassis.addChild("AttachedArm") + + AttachedArm.addObject("MechanicalObject", name = "position", template="Rigid3d", + position=[0., 0.26*1000, 0.32*1000,-0.5, -0.5, -0.5 , 0.5 ], + showObject=True,showObjectScale = 30) + AttachedArm.addObject('RigidRigidMapping',name='mapping', input=self.Chassis.Base.position.getLinkPath(), + index=0) + + trunk = AttachedArm.addChild('Trunk') + base_position = AttachedArm.position + parameters, cables = createEchelon(trunk,base_position,0,[0., 0.26*1000, 0.32*1000],[-90,-90,0]) + + ############### + # TO DO : Réecrire correctement le keybord controller de la trompe + # en mettant le code commenté si dessous dans le fichier + # approprié : CableController + ############## + # if typeControl == 'displacement': + # trunk.addObject(CableController(cables, name = 'Cablecontroller')) + # elif typeControl == 'force' : + # trunk.addObject(ForceController(cables,dt,name = 'ForceController')) + + ########################################## + # add Camera + ########################################## + addCamera(AttachedArm) + return trunk, self, cables + + diff --git a/mobile_trunk_sim/ros_summitxl.py b/mobile_trunk_sim/ros_summitxl.py index 2cec8df..bb4de08 100644 --- a/mobile_trunk_sim/ros_summitxl.py +++ b/mobile_trunk_sim/ros_summitxl.py @@ -1,52 +1,93 @@ -from turtle import position import Sofa +import sys from stlib3.scene import Scene -from splib3.numerics import Quat -from math import pi +from summitxl_roscontroller import * +from stlib3.physics.rigid import Floor +from mobile_trunk import mobileTrunk import sofaros from sofaros import * -from geometry_msgs.msg import Twist -from sensor_msgs.msg import Imu -from summit_xl import SummitXL, Floor -from summitxl_roscontroller import * -from nav_msgs.msg import Odometry + +#from geometry_msgs.msg import Twist +#from sensor_msgs.msg import Imu +#from nav_msgs.msg import Odometry rosNode = sofaros.init("SofaNode") def createScene(rootNode): - scene = Scene(rootNode) + + print("PYTHON: ", sys.argv) + + ######################################### + # Plugins, data and Solvers + ######################################### + rootNode.addObject('RequiredPlugin', name='SofaPython3') + rootNode.addObject('RequiredPlugin', name='BeamAdapter') + rootNode.addObject('RequiredPlugin', name='SoftRobots') + rootNode.addObject('RequiredPlugin', name='SofaMeshCollision') + rootNode.addObject('RequiredPlugin', name='SofaPlugins', pluginName='SofaGeneralRigid SofaGeneralEngine SofaConstraint SofaImplicitOdeSolver SofaSparseSolver SofaDeformable SofaEngine SofaBoundaryCondition SofaRigid SofaTopologyMapping SofaOpenglVisual SofaMiscCollision') + + + scene = Scene(rootNode, iterative=False) scene.addMainHeader() + scene.addContact(alarmDistance=0.2*1000, contactDistance=0.005*1000, frictionCoef=1) + scene.VisualStyle.displayFlags = 'hideBehaviorModels showForceFields showCollisionModels showInteractionForceFields' + scene.addObject('DefaultVisualManagerLoop') scene.dt = 0.01 scene.gravity = [0., -9810., 0.] + scene.Settings.removeObject(scene.Settings.OglSceneFrame) - SummitXL(scene.Modelling) - Floor(scene.Modelling, rotation=[90,0,0], translation=[-2,-0.12,-2], scale=4) - robot=scene.Modelling.SummitXL - scene.Modelling.SummitXL.addObject(SummitxlROSController(name="KeyboardController", robot=scene.Modelling.SummitXL)) + scene.Simulation.addObject('GenericConstraintCorrection', solverName='LinearSolver', ODESolverName='GenericConstraintSolver') + #scene.Simulation.addObject('GenericConstraintCorrection', linearSolver='@LinearSolver', ODESolver='@GenericConstraintSolver') + scene.Simulation.TimeIntegrationSchema.vdamping.value = 0.1 + scene.Simulation.TimeIntegrationSchema.rayleighStiffness = 0.01 + + ######################################### + # add robot + ######################################### + self, trunk= mobileTrunk(scene.Modelling) + scene.Simulation.addChild(trunk) + scene.Simulation.addChild(self) + floor = Floor(rootNode, + name="Floor", + translation=[-2*1000, -0.12*1000, -2*1000], + uniformScale=0.1*1000, + isAStaticObject=True) - scene.Modelling.SummitXL.addObject(sofaros.RosReceiver(rosNode, "/summit_xl/robotnik_base_control/cmd_vel", - [robot.findData('robot_linear_vel'),robot.findData('robot_angular_vel')], - Twist, vel_recv)) + robot=scene.Modelling.SummitXL + + scene.Modelling.SummitXL.addObject(SummitxlROSController(name="KeyboardController", robot=robot)) + scene.Modelling.SummitXL.addObject(sofaros.RosReceiver(rosNode, "/summit_xl/cmd_vel", + [robot.findData('robot_linear_vel'), + robot.findData('robot_angular_vel')], + Twist, vel_recv)) - scene.Modelling.SummitXL.addObject(sofaros.RosSender(rosNode, "/sofa_sim/imu/data",[robot.findData('sim_orientation'), - robot.findData('robot_angular_vel'), robot.findData('linear_acceleration'), - robot.findData('timestamp')],Imu, send)) + #scene.Modelling.SummitXL.addObject( + # sofaros.RosSender(rosNode, "/sofa_sim/imu/data", [robot.findData('sim_orientation'), + # robot.findData('robot_angular_vel'), + # robot.findData('linear_acceleration'), + # robot.findData('timestamp')], Imu, send)) - scene.Modelling.SummitXL.addObject(sofaros.RosSender(rosNode, "/sofa_sim/odom",[robot.findData('timestamp'), - robot.findData('sim_position'), robot.findData('sim_orientation'), - robot.findData('robot_linear_vel'), robot.findData('robot_angular_vel')], - Odometry, odom_send)) + # scene.Modelling.SummitXL.addObject(sofaros.RosSender(rosNode, "/summit_xl/robotnik_base_control/odom", + # [robot.findData('timestamp'), + # robot.findData('sim_position'), + # robot.findData('sim_orientation'), + # robot.findData('robot_linear_vel'), + # robot.findData('robot_angular_vel')], + # Odometry, odom_send)) - scene.Modelling.SummitXL.addObject(sofaros.RosReceiver(rosNode, "/summit_xl/robotnik_base_control/odom",[robot.findData('timestamp'), - robot.findData('reel_position'), robot.findData('reel_orientation')], - Odometry, odom_recv)) + scene.Modelling.SummitXL.addObject( + sofaros.RosReceiver(rosNode, "/summit_xl/robotnik_base_control/odom", [robot.findData('timestamp'), + robot.findData('reel_position'), + robot.findData('reel_orientation'), + robot.findData('robot_linear_vel'), + robot.fincData('robot_angular_vel')], + Odometry, odom_recv)) - scene.Modelling.SummitXL.addObject(sofaros.RosSender(rosNode, "/sofa_sim/cmd_vel", + scene.Modelling.SummitXL.addObject(sofaros.RosSender(rosNode, "/summit_xl/robotnik_base_control/cmd_vel", [robot.findData('robot_linear_vel'),robot.findData('robot_angular_vel')], Twist, vel_send)) - scene.Simulation.addChild(scene.Modelling) return rootNode diff --git a/mobile_trunk_sim/scene.py b/mobile_trunk_sim/scene.py new file mode 100644 index 0000000..9f61bff --- /dev/null +++ b/mobile_trunk_sim/scene.py @@ -0,0 +1,124 @@ +import Sofa +import sys +from stlib3.scene import Scene +from summitxl_controller import * +from summitxl_roscontroller import * +from echelon3.RosCableController import * +from stlib3.physics.rigid import Floor +#from floor import Floor +from mobile_trunk import mobileTrunk +from addCamera import addCamera + + + +def createScene(rootNode): + + ######################################### + # Plugins, data and Solvers + ######################################### + + rootNode.addObject('RequiredPlugin', name='SofaPython3') + rootNode.addObject('RequiredPlugin', name='BeamAdapter') + rootNode.addObject('RequiredPlugin', name='SoftRobots') + rootNode.addObject('RequiredPlugin', name='SofaMeshCollision') + rootNode.addObject('RequiredPlugin', name='SofaPlugins', pluginName='SofaGeneralRigid SofaGeneralEngine SofaConstraint SofaImplicitOdeSolver SofaSparseSolver SofaDeformable SofaEngine SofaBoundaryCondition SofaRigid SofaTopologyMapping SofaOpenglVisual SofaMiscCollision') + + + scene = Scene(rootNode, iterative=False) + scene.addMainHeader() + scene.addContact(alarmDistance=0.1*1000, contactDistance=0.05*1000, frictionCoef=8) + scene.VisualStyle.displayFlags = 'hideBehaviorModels showForceFields showCollisionModels showInteractionForceFields' + scene.addObject('DefaultVisualManagerLoop') + scene.dt = 0.01 + scene.gravity = [0., -9810., 0.] + scene.Settings.removeObject(scene.Settings.OglSceneFrame) + + scene.Simulation.addObject('GenericConstraintCorrection', solverName='LinearSolver', ODESolverName='GenericConstraintSolver') + + #scene.Simulation.addObject('GenericConstraintCorrection', linearSolver='@LinearSolver', ODESolver='@GenericConstraintSolver') + scene.Simulation.TimeIntegrationSchema.vdamping.value = 0.1 + scene.Simulation.TimeIntegrationSchema.rayleighStiffness = 0.01 + + ######################################### + # add robot + ######################################### + scale = 500 + self, trunk, cables = mobileTrunk(scene.Modelling) + scene.Simulation.addChild(trunk) + scene.Simulation.addChild(self) + + floor = Floor(rootNode, + name="Floor", + translation=[-2*scale, -0.1*scale, 2*scale], + uniformScale=0.5*scale, + isAStaticObject=True) + + #def myAnimation(target, body, factor): + # body.position += [[0.0,0.0,0.001,0.0,0,0,1]] + # target.position = [[factor* 3.14 * 2]]*len(target.position.value) + + #animate(myAnimation, { + # "body" : scene.Modelling.SummitXL.Chassis.position, + # "target": scene.Modelling.SummitXL.Chassis.WheelsMotors.angles}, duration=2, mode="loop") + + if sys.argv[1] == "KeyboardController": + scene.Modelling.SummitXL.addObject(SummitxlController(name="KeyboardController", robot=scene.Modelling.SummitXL, test=False)) + + elif sys.argv[1] == "SimToRobot": + import sofaros + from sofaros import RosReceiver + rosNode = sofaros.init("SofaNode") + robot=scene.Modelling.SummitXL + scene.Modelling.SummitXL.addObject(SummitxlROSController(name="KeyboardController", robot=robot, robotToSim=False, test=False)) + + # scene.Modelling.SummitXL.addObject(sofaros.RosSender(rosNode, "/summit_xl/robotnik_base_control/odom", + # [robot.findData('timestamp'), + # robot.findData('sim_position'), + # robot.findData('sim_orientation'), + # robot.findData('robot_linear_vel'), + # robot.findData('robot_angular_vel')], + # Odometry, odom_send)) + + # scene.Modelling.SummitXL.addObject(sofaros.RosSender(rosNode, "/summit_xl/robotnik_base_control/cmd_vel", + # [robot.findData('robot_linear_vel'),robot.findData('robot_angular_vel')], + # Twist, vel_send)) + scene.Modelling.SummitXL.addObject(RosReceiver(rosNode, "/summit_xl/robotnik_base_control/cmd_vel", [robot.findData('robot_linear_vel'), + robot.findData('robot_angular_vel')], + Twist, vel_recv)) + + elif sys.argv[1] == "RobotToSim": + import sofaros + from sofaros import RosReceiver, RosSender + rosNode = sofaros.init("SofaNode") + robot=scene.Modelling.SummitXL + scene.Modelling.SummitXL.addObject(SummitxlROSController(name="KeyboardController", robot=robot, robotToSim=True, test=False)) + scene.Modelling.SummitXL.addObject(RosReceiver(rosNode, "/summit_xl/robotnik_base_control/odom", + [robot.findData('timestamp'), + robot.findData('reel_position'), + robot.findData('reel_orientation'), + robot.findData('robot_linear_vel'), + robot.findData('robot_angular_vel')], + Odometry, odom_recv)) + + scene.Modelling.SummitXL.addObject(RosSender(rosNode, "/digital_twin/odom", [robot.findData('sim_position'), + robot.findData('sim_orientation')],Odometry, + position_and_orientation_send)) + + scene.Modelling.SummitXL.addObject(RosSender(rosNode, "/digital_twin/joint_states", robot.findData('digital_twin_joints_states_vel'), + sensor_msgs.msg.JointState, digital_twin_jointstate_pub)) + + scene.Modelling.SummitXL.addObject(RosReceiver(rosNode, "/summit_xl/joint_states", robot.findData('summit_xl_joints_states_vel'), + sensor_msgs.msg.JointState, summit_xl_jointstate_recv)) + + + scene.Modelling.SummitXL.addObject(CableROSController(name="CableROSController", rosNode = rosNode, cables=cables, robot=robot, robotToSim=True, test=False)) + + for i in range(1,10): + + topic = str("/Robot/Cable"+str(i)+"/state/displacement") + scene.Modelling.SummitXL.addObject(RosReceiver(rosNode, topic, robot.findData("effector_cable_data"),Float64, cable_displacement_recv)) + print(topic) + + scene.Modelling.SummitXL.addObject(RosSender(rosNode, "/Robot/end_effector/pos", robot.findData("end_effector_pos"),Float64MultiArray, end_effector_pos_send)) + + return rootNode \ No newline at end of file diff --git a/mobile_trunk_sim/scene1.py b/mobile_trunk_sim/scene1.py new file mode 100644 index 0000000..af97ae5 --- /dev/null +++ b/mobile_trunk_sim/scene1.py @@ -0,0 +1,75 @@ +import sys +import Sofa +from stlib3.physics.rigid import Floor +from mobile_trunk import mobileTrunk +from summitxl_controller import * + + + +def createScene(rootNode): + + + ######################################### + # Plugins, data and Solvers + ######################################### + + rootNode.addObject('RequiredPlugin', name='SofaPython3') + rootNode.addObject('RequiredPlugin', name='BeamAdapter') + rootNode.addObject('RequiredPlugin', name='SoftRobots') + rootNode.addObject('RequiredPlugin', name='SofaMeshCollision') + rootNode.addObject('RequiredPlugin', name='SofaPlugins', pluginName='SofaGeneralRigid SofaGeneralEngine SofaConstraint SofaImplicitOdeSolver SofaSparseSolver SofaDeformable SofaEngine SofaBoundaryCondition SofaRigid SofaTopologyMapping SofaOpenglVisual SofaMiscCollision') + + + scene = Scene(rootNode, iterative=False) + scene.addMainHeader() + scene.addContact(alarmDistance=0.1*1000, contactDistance=0.05*1000, frictionCoef=8) + scene.VisualStyle.displayFlags = 'hideBehaviorModels showForceFields showCollisionModels showInteractionForceFields' + scene.addObject('DefaultVisualManagerLoop') + scene.dt = 0.01 + scene.gravity = [0., -9810., 0.] + scene.Settings.removeObject(scene.Settings.OglSceneFrame) + + scene.Simulation.addObject('GenericConstraintCorrection', solverName='LinearSolver', ODESolverName='GenericConstraintSolver') + + scene.Simulation.TimeIntegrationSchema.vdamping.value = 0.1 + scene.Simulation.TimeIntegrationSchema.rayleighStiffness = 0.01 + + ######################################### + # add robot + ######################################### + self, trunk= mobileTrunk(scene.Modelling) + scene.Simulation.addChild(trunk) + scene.Simulation.addChild(self) + + floor = Floor(rootNode, + name="Floor", + translation=[-2*1000, -0.1*1000, 2*1000], + uniformScale=0.1*1000, + isAStaticObject=True) + + print(sys.argv[1]) + arg = sys.argv[1] + arg = arg.split() + print(arg) + + + robot_angular_speed = float(arg[0]) + robot_linear_speed = float(arg[1]) + sim_duration = float(arg[2]) + scene.Modelling.SummitXL.addObject(SummitxlController(name="KeyboardController", robot=scene.Modelling.SummitXL, + angular_speed = robot_angular_speed, + linear_speed = robot_linear_speed, + duration = sim_duration, test= True) + ) + + #print(sys.argv) + #def myAnimation(target, body, factor): + # body.position += [[0.0,0.0,0.001,0.0,0,0,1]] + # target.position = [[factor* 3.14 * 2]]*len(target.position.value) + + #animate(myAnimation, { + # "body" : scene.Modelling.SummitXL.Chassis.position, + # "target": scene.Modelling.SummitXL.Chassis.WheelsMotors.angles}, duration=2, mode="loop") + + + return rootNode \ No newline at end of file diff --git a/mobile_trunk_sim/summit_xl.py b/mobile_trunk_sim/summit_xl.py index e7f9c0e..e32a4fc 100644 --- a/mobile_trunk_sim/summit_xl.py +++ b/mobile_trunk_sim/summit_xl.py @@ -1,60 +1,71 @@ -from turtle import position import Sofa + from stlib3.scene import Scene -from splib3.numerics import Quat -from math import pi +from stlib3.scene import ContactHeader +from stlib3.physics.rigid import Floor + from summitxl_controller import * +from echelon3.parameters import * +from echelon3.createEchelon import * + def Chassis(): - """The summitXL chassis description. - The chassis is composed of: - - a rigid frame for its main position - - four wheels motors each with an angle describing the angular position of each wheel. - - 3 sensors linked to the chassis - *lazer - *imu - *camera - """ + + ######################################### + # parameters + ######################################### + totalMass = 1.0 + volume = 1.0*1e9 + inertiaMatrix = [1.0*1e6, 0.0, 0.0, 0.0, 1.0*1e6, 0.0, 0.0, 0.0, 1.0*1e6] + + wheelPositions = [[0.229*1000, 0,0.235*1000], + [-0.229*1000, 0,0.235*1000], + [0.229*1000, 0,-0.235*1000], + [-0.229*1000, 0,-0.235*1000]] + + sensorPositions = [[0., 0.28*1000 ,0.], # 2d lazer + [0,0.275*1000,-0.22*1000], # gps + ] + + sensorName=["lazer", "gps"] + + trunkPosition = [0., 0.26*1000, 0.32*1000,-0.5, -0.5, -0.5 , 0.5 ] + + ######################################### + #Add chassis mechanical Object + ######################################### + self = Sofa.Core.Node("Chassis") - self.addObject("MechanicalObject", name="position", template="Rigid3d", position=[[0,0,0,0,0,0,1]]) - - #debug - debug = self.addChild("Debug") - debug.addObject("MechanicalObject", name="position", template="Rigid3d", position=self.position.position.value) - debug.position.showObject = True - debug.position.showObjectScale = 0.5 - debug.position.drawMode = 1 - debug.position.showColor = [0,1,0,1] - - reel_robot = self.addChild("Reel_robot") - reel_robot.addObject("MechanicalObject", name="position", template="Rigid3d", position=self.position.position.value) - reel_robot.position.showObject = True - reel_robot.position.showObjectScale = 0.5 - reel_robot.position.drawMode = 1 - reel_robot.position.showColor =[10., 8., 2., 0.75] - # + + base = self.addChild('Base') + base.addObject("MechanicalObject", name="position", template="Rigid3d", position=[[0,0,0,0,0,0,1]]) + base.addObject('UniformMass', name="vertexMass", vertexMass=[totalMass, volume, inertiaMatrix[:]]) + base.addObject('UncoupledConstraintCorrection') + + ######################################### + #creation of the articulated chain of wheels, sensors + ######################################### + + #wheels chain = self.addChild("WheelsMotors") chain.addObject('MechanicalObject', name="angles", template="Vec1d", position=[0,0,0,0,0]) + chain.addObject('UniformMass', name="vertexMass", vertexMass=[totalMass, volume, inertiaMatrix[:]]) + chain.addObject('RestShapeSpringsForceField', stiffness=1e12) + + #capteur + sensor = self.addChild("FixedSensor") + sensor.addObject('MechanicalObject', name="angles", template="Vec1d", position=[0,0,0]) + sensor.addObject('UniformMass', name="vertexMass", vertexMass=[totalMass, volume, inertiaMatrix[:]]) - sensor = self.addChild("FixedSensor") - sensor.addObject('MechanicalObject', name="angles", template="Vec1d", position=[0,0,0,0,0]) + ######################################### + #description of the articulated chain from the chassis'position to the wheels and the sensors + ######################################### - ## The following is needed to describe the articulated chain from the chass's position - ## to the wheel's and the sensor's one . chain.addObject('ArticulatedHierarchyContainer') - wheelPositions = [[0.229, 0,0.235], - [-0.229, 0,0.235], - [0.229, 0,-0.235], - [-0.229, 0,-0.235]] - sensorName=["lazer", "gps", "camera", "camera_RGBD"] + sensor.addObject('ArticulatedHierarchyContainer') - sensorPositions = [[0., 0.28 ,0.], # 2d lazer - [0,0.275,-0.22], # gps - [0., 0.26, 0.19], # front_ptz_camera - [0.012, 0.172, 0.324] # front_rgbd_camera - ] - for i in range(4): + for i in range(2): sc = sensor.addChild(sensorName[i]) sc.addObject('ArticulationCenter', parentIndex=0, childIndex=1+i, posOnParent=sensorPositions[i]) s = sc.addChild("Articulation") @@ -66,28 +77,46 @@ def Chassis(): a = ac.addChild("Articulation") a.addObject('Articulation', translation=False, rotation=True, rotationAxis=[1, 0, 0], articulationIndex=i) - wheels = self.addChild("Wheels") - # There is one extra position in this mechanical object because there the articulated chain - # Needs a root one (in addition to the four wheels) + ############################## + #There is one extra position in this mechanical object because there + #the articulated chain Needs a root one (in addition to the four wheels) + ############################## + + wheels = base.addChild("Wheels") + chain.addChild(wheels) + wheels.addObject("MechanicalObject", name="position", template="Rigid3d", position=[[0,0,0,0,0,0,1], [0,0,0,0,0,0,1], [0,0,0,0,0,0,1], [0,0,0,0,0,0,1], [0,0,0,0,0,0,1]], showObject=True) + wheels.addObject('UniformMass', name="vertexMass", vertexMass=[totalMass, volume, inertiaMatrix[:]]) wheels.addObject('ArticulatedSystemMapping', input1=chain.angles.getLinkPath(), - input2=self.position.getLinkPath(), + input2=base.position.getLinkPath(), output=wheels.position.getLinkPath()) - #Add sensors - sensors = self.addChild("Sensors") + + ######################################### + # add sensors + ######################################### + + sensors = base.addChild("Sensors") + sensor.addChild(sensors) + sensors.addObject("MechanicalObject", name = "position", template="Rigid3d", - position=[[0,0,0,0,0,0,1], [0,0,0,0,0,0,1], [0,0,0,0,0,0,1], [0,0,0,0,0,0,1], [0,0,0,0,0,0,1]], + position=[[0,0,0,0,0,0,1], [0,0,0,0,0,0,1], [0,0,0,0,0,0,1]], showObject=True) + sensors.addObject('UniformMass', name="vertexMass", vertexMass=[totalMass, volume, inertiaMatrix[:]]) + sensors.addObject('ArticulatedSystemMapping', input1=sensor.angles.getLinkPath(), - input2=self.position.getLinkPath(), + input2=base.position.getLinkPath(), output=sensors.position.getLinkPath()) - ## Adds VisualModel for the chassis's body + ######################################### + # visual models + ######################################### + + ## Chassis's body visual = self.addChild("VisualModel") parts = { "Chassis" : ('meshes/summit_xl_chassis.stl', [0.1,0.1,0.1,1.0]) , @@ -96,38 +125,63 @@ def Chassis(): } for name, (filepath, color) in parts.items(): part = visual.addChild(name) - part.addObject('MeshSTLLoader', name='loader', filename=filepath, rotation=[-90,-90,0]) + part.addObject('MeshSTLLoader', name='loader', filename=filepath, rotation=[-90,-90,0],scale3d = [1000,1000,1000]) part.addObject('MeshTopology', src='@loader') part.addObject('OglModel', name="renderer", src='@loader', color=color) - part.addObject('RigidMapping', input=self.Wheels.position.getLinkPath(), index=0) + part.addObject('RigidMapping', input=wheels.position.getLinkPath(), index=0) - ## Add VisualModel for the wheels + ## Wheels visual = wheels.addChild("VisualModel") - visual.addObject('MeshSTLLoader', name='loader', filename='meshes/wheel.stl', rotation=[0,0,90]) + visual.addObject('MeshSTLLoader', name='loader', filename='meshes/wheel.stl', rotation=[0,0,90],scale3d = [1000,1000,1000]) visual.addObject('MeshTopology', name='geometry', src='@loader') for i in range(4): wheel = visual.addChild("Wheel{0}".format(i)) wheel.addObject("OglModel", src=visual.geometry.getLinkPath(), color=[0.2,0.2,0.2,1.0]) - wheel.addObject("RigidMapping", input=self.Wheels.position.getLinkPath(), index=i+1) - + wheel.addObject("RigidMapping", input=wheels.position.getLinkPath(), index=i+1) - ## Add VisualModel for the sensors + ## Sensors visual = sensors.addChild("VisualModel") sensorfilepath = { "lazer" : ('meshes/hokuyo_urg_04lx.stl', 1) , - "gps" : ('meshes/antenna_3GO16.stl', 2), - "camera" : ('meshes/axis_p5514.stl',3), - "camera-RGBD" : ('meshes/orbbec_astra_embedded_s.stl', 4) + "gps" : ('meshes/antenna_3GO16.stl', 2) } + for name, (filepath, index) in sensorfilepath.items(): visual_body = visual.addChild(name) - visual_body.addObject('MeshSTLLoader', name=name+'_loader', filename=filepath, rotation=[0,90,90]) + visual_body.addObject('MeshSTLLoader', name=name+'_loader', filename=filepath, rotation=[0,90,90],scale3d = [1000,1000,1000]) visual_body.addObject('MeshTopology', src='@'+name+'_loader') visual_body.addObject('OglModel', name=name+"_renderer", src='@'+name+'_loader', color=[0.2,0.2,0.2,1.0]) - visual_body.addObject('RigidMapping', input=self.Sensors.position.getLinkPath(),index=index) + visual_body.addObject('RigidMapping', input=sensors.position.getLinkPath(),index=index) + + ######################################### + # collision models + ######################################### + + collison_model = wheels.addChild("CollisionModel") + for i in range(4): + wheel_collision = collison_model.addChild("WheelCollision{0}".format(i)) + wheel_collision.addObject('MeshSTLLoader', name='loader', filename='meshes/collision_wheel.stl', rotation=[0, 90, 0],scale3d = [1000,1000,1000]) + wheel_collision.addObject('MeshTopology', src='@loader') + wheel_collision.addObject('MechanicalObject') + wheel_collision.addObject('TriangleCollisionModel', group=0) + wheel_collision.addObject('LineCollisionModel',group=0) + wheel_collision.addObject('PointCollisionModel', group=0) + wheel_collision.addObject('RigidMapping', input=wheels.position.getLinkPath(), index=i+1) + + ######################################### + # add Trunk + ######################################### + + # trunk = base.addChild("Trunk") + # trunk.addObject("MechanicalObject", name = "position", template="Rigid3d", + # position=trunkPosition, + # showObject=True,showObjectScale = 30) + # trunk.addObject('RigidRigidMapping',name='mapping', input=base.position.getLinkPath(), index=0) + return self + def SummitXL(parentNode, name="SummitXL"): self = parentNode.addChild(name) self.addData(name="robot_linear_vel", value=[0.0, 0.0, 0.0], @@ -146,36 +200,67 @@ def SummitXL(parentNode, name="SummitXL"): type="Vec3d", help="Summit_xl imu", group="Summitxl_cmd_vel") self.addData(name="timestamp",value=[0, 0], type="vector", help="Summit_xl imu", - group="Summitxl_cmd_vel") + group="Summit_xl_cmd_vel") self.addData(name="sim_position", value=[0.0, 0.0, 0.0],type="Vec3d", help="Summit_xl odom", group="Summitxl_cmd_vel") self.addData(name="reel_position", value=[0.0, 0.0, 0.0],type="Vec3d", help="Summit_xl odom", group="Summitxl_cmd_vel") - + + self.addData(name="digital_twin_joints_states_vel",value=[0, 0, 0, 0], type="Vec4d", help="Summit_xl joint_states", + group="Summit_xl_cmd_vel") + + self.addData(name="summit_xl_joints_states_vel",value=[0, 0, 0, 0], type="Vec4d", help="Summit_xl joint_states", + group="Summit_xl_cmd_vel") + + self.addData(name="effector_cable_data", type="float", help="trunk displacement", group="Echelon3") + + self.addData(name="end_effector_pos", value = [0,0,0,0,0,0,0],type="vector", help="trunk displacement", group="Echelon3") + + self.addChild(Chassis()) return self -def Floor(parentNode, color=[0.5, 0.5, 0.5, 1.], rotation=[0, 0, 0], translation=[0, 0, 0], scale=1): - floor = parentNode.addChild('Floor') - floor.addObject('MeshObjLoader', name='loader', filename='mesh/square1.obj', scale=scale, rotation=rotation, translation=translation) - floor.addObject('OglModel', src='@loader', color=color) - floor.addObject('MeshTopology', src='@loader', name='topo') - floor.addObject('MechanicalObject') - floor.addObject('TriangleCollisionModel') - floor.addObject('LineCollisionModel') - floor.addObject('PointCollisionModel') - return floor - def createScene(rootNode): - scene = Scene(rootNode) + + #ContactHeader(rootNode, alarmDistance=0.2*1000, contactDistance=0.005*1000) + + ######################################### + # Plugins, data and Solvers + ######################################### + + rootNode.addObject('OglSceneFrame', style="Arrows", alignment="TopRight"); + rootNode.addObject('RequiredPlugin', name='SofaPython3') + rootNode.addObject('RequiredPlugin', name='BeamAdapter') + rootNode.addObject('RequiredPlugin', name='SoftRobots') + rootNode.addObject('RequiredPlugin', name='SofaMeshCollision') + rootNode.addObject('RequiredPlugin', name='SofaPlugins', pluginName='SofaGeneralRigid SofaGeneralEngine SofaConstraint SofaImplicitOdeSolver SofaSparseSolver SofaDeformable SofaEngine SofaBoundaryCondition SofaRigid SofaTopologyMapping SofaOpenglVisual SofaMiscCollision') + + + scene = Scene(rootNode, iterative=False) scene.addMainHeader() - scene.dt = 0.001 + scene.addContact(alarmDistance=0.2*1000, contactDistance=0.005*1000, frictionCoef=0.8) + scene.VisualStyle.displayFlags = 'hideBehaviorModels showForceFields showCollisionModels showInteractionForceFields' + scene.addObject('DefaultVisualManagerLoop') + scene.dt = 0.01 scene.gravity = [0., -9810., 0.] + scene.Simulation.addObject('GenericConstraintCorrection', solverName='LinearSolver', ODESolverName='GenericConstraintSolver') + + scene.Simulation.TimeIntegrationSchema.vdamping.value = 0.1 + scene.Simulation.TimeIntegrationSchema.rayleighStiffness = 0.01 + + ######################################### + # create summit + ######################################### + scene.Simulation.addChild(scene.Modelling) SummitXL(scene.Modelling) - Floor(scene.Modelling, rotation=[90,0,0], translation=[-2,-0.12,-2], scale=4) + floor = Floor(rootNode, + name="Floor", + translation=[-2*1000, -0.12*1000, -2*1000], + uniformScale=0.1*1000, + isAStaticObject=True) #def myAnimation(target, body, factor): # body.position += [[0.0,0.0,0.001,0.0,0,0,1]] @@ -185,8 +270,18 @@ def createScene(rootNode): # "body" : scene.Modelling.SummitXL.Chassis.position, # "target": scene.Modelling.SummitXL.Chassis.WheelsMotors.angles}, duration=2, mode="loop") - scene.Modelling.SummitXL.addObject(SummitxlController(name="KeyboardController", robot=scene.Modelling.SummitXL)) + scene.Modelling.SummitXL.addObject(SummitxlController(name="KeyboardController", robot=scene.Modelling.SummitXL, test=False)) - scene.Simulation.addChild(scene.Modelling) + ######################################## + # createEchelon + ######################################## + + # arm = rootNode.Modelling.SummitXL.Chassis.addChild('Arm') + # connection = rootNode.Modelling.SummitXL.Chassis.Base.Trunk.position + # parameters, cables = createEchelon(arm,connection,0,[0., 0.26*1000, 0.32*1000],[-90,-90,0]) - return rootNode + # if typeControl == 'displacement': + # arm.addObject(CableController(cables, name= 'Cablecontroller')) + # elif typeControl == 'force': + # arm.addObject(ForceController(cables, dt, name = 'ForceController')) + return rootNode \ No newline at end of file diff --git a/mobile_trunk_sim/summitxl_controller.py b/mobile_trunk_sim/summitxl_controller.py index a45dbea..2ceed09 100644 --- a/mobile_trunk_sim/summitxl_controller.py +++ b/mobile_trunk_sim/summitxl_controller.py @@ -4,6 +4,16 @@ from splib3.constants import Key from stlib3.scene import Scene from math import * +from wheels_angles_compute import twistToWheelsAngularSpeed, move +from functional_test import test +import matplotlib.pyplot as plt + + +time_data = [] +pos_data = [] +reel_pos_data = [] +angle_data = [] +reel_angle_data = [] msg = """ This node takes keypresses from the keyboard and publishes them @@ -25,7 +35,7 @@ moveBindings = { 'i': (1, 0, 0, 0), - 'p': (1, 0, 0, -1), + 'p': (-1, 0, 0, -1), 'j': (0, 0, 0, 1), 'l': (0, 0, 0, -1), 'u': (1, 0, 0, 1), @@ -35,16 +45,17 @@ } speedBindings = { - 'q': (1.1, 1.1), - 'z': (.9, .9), - 'w': (1.1, 1), - 'x': (.9, 1), - 'e': (1, 1.1), - 'c': (1, .9), + 'q': (0.1, 0.1), + 'z': (-0.1, -0.1), + 'w': (0.1, 0), + 'x': (-0.1, 0), + 'e': (0, 0.1), + 'c': (0, -0.1), } def vels(speed, turn): return 'currently:\tspeed %s\tturn %s ' % (speed, turn) + class SummitxlController(Sofa.Core.Controller): """A Simple keyboard controller for the SummitXL Key UP, DOWN, LEFT, RIGHT to move @@ -52,76 +63,97 @@ class SummitxlController(Sofa.Core.Controller): def __init__(self, *args, **kwargs): Sofa.Core.Controller.__init__(self, *args, **kwargs) self.robot = kwargs["robot"] - self.wheel_ray = 0.0015 + self.test_flag = kwargs["test"] + if self.test_flag : + self.test_angular_speed = kwargs["angular_speed"] + self.test_linear_speed = kwargs["linear_speed"] + self.test_duration = kwargs["duration"] self.dt = 0 - self.status = 0. - self.speed = 0.1 - self.turn = 0.1 + self.speed = 1 + self.turn = 2 self.x = 0.0 self.y = 0.0 self.z = 0.0 self.th = 0.0 self.robot.simrobot_angular_vel = [0., 0., 0.] self.robot.simrobot_linear_vel = [0., 0., 0.] + self.elapsed_time = 0 + self.deplacement_ctrl = 0 + self.angle_ctrl = 0 + self.start = False + self.w = 0 + self.robot_angle_rotation = [0,0,0,0] + self.epsilon = 0 - def move(self, fwd, angle): - """Move the robot using the forward speed and angular speed)""" - robot = RigidDof(self.robot.Chassis.position) - robot.translate(robot.forward * fwd) - robot.rotateAround([0, 1, 0], angle) - - with self.robot.Chassis.WheelsMotors.angles.position.writeable() as angles: - #Make the wheel turn according to forward speed - # TODO: All the value are random, need to be really calculated - angles += (fwd/self.wheel_ray) - #Make the wheel turn in reverse mode according to turning speed - # TODO: the value are random, need to be really calculated - angles[0] += (angle) - angles[2] += (angle) - angles[1] -= (angle) - angles[3] -= (angle) def onAnimateBeginEvent(self, event): """At each time step we move the robot by the given forward_speed and angular_speed) - TODO: normalize the speed by the dt so it is a real speed """ + self.dt = event['dt'] - self.move(self.robot.simrobot_linear_vel[0] , self.robot.simrobot_angular_vel[2]) - print("position x = ",self.robot.Chassis.position.position.value[0][0]) - print("position y = ",self.robot.Chassis.position.position.value[0][1]) - print("position z = ",self.robot.Chassis.position.position.value[0][2]) - print("\n") - print("orientation x = ",self.robot.Chassis.position.position.value[0][3]) - print("orientation y = ",self.robot.Chassis.position.position.value[0][4]) - print("orientation z = ",self.robot.Chassis.position.position.value[0][5]) - print("orientation w = ",self.robot.Chassis.position.position.value[0][6]) - - - - - + if not self.test_flag: + wheels_angular_speed = twistToWheelsAngularSpeed(self.robot.simrobot_angular_vel[2], + self.robot.simrobot_linear_vel[0]) + move(self.robot.Chassis.WheelsMotors.angles.rest_position, + wheels_angular_speed, self.dt) + + elif self.test_flag and self.start: + + self.elapsed_time +=self.dt + #0.65793165467 + self.angle_ctrl +=0.628319 * self.dt + self.deplacement_ctrl +=self.test_linear_speed * self.dt + + time_data.append(self.elapsed_time) + pos_data.append(self.deplacement_ctrl) + reel_pos_data.append(self.robot.Chassis.Base.position.position.value[0][2]/1000) + angle_data.append(self.angle_ctrl) + + q = Quat(self.robot.Chassis.Base.position.position.value[0][3], + self.robot.Chassis.Base.position.position.value[0][4], + self.robot.Chassis.Base.position.position.value[0][5], + self.robot.Chassis.Base.position.position.value[0][6]) + self.robot_angle_rotation = q.getEulerAngles() + print("y rotation = ",self.robot_angle_rotation, "angle_ctrl = ", self.angle_ctrl) + + if self.elapsed_time >= self.test_duration: + self.test_linear_speed = 0 + self.test_angular_speed = 0 + final_pos =[self.robot.Chassis.Base.position.position.value[0][0], + self.robot.Chassis.Base.position.position.value[0][1], + self.robot.Chassis.Base.position.position.value[0][2]] + self.start = False + + self.wheels_angular_speed = twistToWheelsAngularSpeed(self.test_angular_speed, self.test_linear_speed) + wheels_rotation_value = move(self.robot.Chassis.WheelsMotors.angles.rest_position, self.wheels_angular_speed, self.dt) + self.w+= wheels_rotation_value[1][0] + #print("-------->", wheels_rotation_value[1][0]) + reel_angle_data.append(wheels_rotation_value[1][0]) + # reel_angle_data.append(self.robot_angle_rotation[1]) + + self.plot() def onKeypressedEvent(self, event): key = event['key'] key = key.lower() + if Key.space: + self.start = True if key in moveBindings.keys(): self.x = moveBindings[key][0] self.th = moveBindings[key][3] + self.robot.simrobot_linear_vel[0] = self.x * self.speed + self.robot.simrobot_angular_vel[2] = self.th * self.turn elif key in speedBindings.keys(): self.speed = self.speed + speedBindings[key][0] - self.turn = self.speed + speedBindings[key][1] + self.turn = self.turn + speedBindings[key][1] - print(vels(self.speed, self.turn)) if (self.status == 14): print(msg) self.status = (self.status + 1) % 15 else: self.x = 0.0 self.th = 0.0 - self.robot.simrobot_linear_vel[0] = self.x * self.speed * self.dt - self.robot.simrobot_angular_vel[2] = self.th * self.turn * self.dt - def onKeyreleasedEvent(self, event): @@ -130,3 +162,22 @@ def onKeyreleasedEvent(self, event): if key in moveBindings.keys() or key in speedBindings.keys(): self.robot.simrobot_linear_vel[0]= 0 self.robot.simrobot_angular_vel[2] = 0 + + def plot(self): + if not self.start: + print('-------------------') + print( 'Erreur = ',abs(reel_pos_data[-1] - pos_data[-1])) + # plt.plot(time_data, reel_angle_data, "k.", label="angle de rotation d'une roue donnée par sofa= f(time_data)") + # plt.plot(time_data, angle_data ,"r," ,label="angle de rotation d'une roue calculé = f(time_data)") + plt.plot(time_data, reel_pos_data , label="reel_pos_data = f(time_data)") + plt.plot(time_data, pos_data , label="pos_data= f(time_data)") + plt.xlabel('x - axis') + # naming the y axis + plt.ylabel('y - axis') + + # giving a title to my graph + # plt.title() + + plt.legend() + plt.show() + diff --git a/mobile_trunk_sim/summitxl_roscontroller.py b/mobile_trunk_sim/summitxl_roscontroller.py index 137ea61..16d46d6 100644 --- a/mobile_trunk_sim/summitxl_roscontroller.py +++ b/mobile_trunk_sim/summitxl_roscontroller.py @@ -1,12 +1,18 @@ # coding: utf8 #!/usr/bin/env python3 +import numpy as np import Sofa -from splib3.numerics import RigidDof from sensor_msgs.msg import Imu -from geometry_msgs.msg import Twist,Point, Pose, Quaternion, Twist, Vector3 +from geometry_msgs.msg import Twist +from wheels_angles_compute import twistToWheelsAngularSpeed, move, updateOdometry from nav_msgs.msg import Odometry +import sensor_msgs.msg import time from math import * +import queue +import json + +q = queue.Queue() def send(data): """This is a message to hold data from an IMU (Inertial Measurement Unit) @@ -73,7 +79,7 @@ def vel_recv(data, datafield): velocity """ datafield[0].value = [data.linear.x ,data.linear.y, data.linear.z] - datafield[1].value = [data.angular.x ,data.angular.z, data.angular.y] + datafield[1].value = [data.angular.x ,data.angular.y, data.angular.z] def odom_recv(data, datafield): """ Function called by the RosReceiver. It @@ -100,7 +106,39 @@ def odom_recv(data, datafield): datafield[2].value = [data.pose.pose.orientation.x,data.pose.pose.orientation.z, data.pose.pose.orientation.y,data.pose.pose.orientation.w ] + datafield[3].value = [data.twist.twist.linear.x, 0 ,0] + datafield[4].value = [0, 0, data.twist.twist.angular.z] + + +def position_and_orientation_send(data): + msg = Odometry() + #odom position x y z + msg.pose.pose.position.x = data[0].value[2]/1000 + msg.pose.pose.position.z = data[0].value[1]/1000 + msg.pose.pose.position.y = data[0].value[0]/1000 + + #odom orientation x y z w + msg.pose.pose.orientation.x = data[1].value[0] + msg.pose.pose.orientation.z = data[1].value[1] + msg.pose.pose.orientation.y = data[1].value[2] + msg.pose.pose.orientation.w = data[1].value[3] + #print(msg.pose.pose.position) + return msg + +def digital_twin_jointstate_pub(data): + + joint_state= sensor_msgs.msg.JointState() + joint_state.name = ["digital_twin_front_left_wheel_joint", "digital_twin_front_right_wheel_joint", + "digital_twin_back_left_wheel_joint", "digital_twin_back_right_wheel_joint"] + joint_state.effort = [0., 0., 0., 0.] + joint_state.position = [0., 0., 0., 0.] + + joint_state.velocity = [data.value[0], data.value[1], data.value[2], data.value[3]] + return joint_state + +def summit_xl_jointstate_recv(data, datafield): + datafield.value = [data.velocity[0], data.velocity[1], data.velocity[2], data.velocity[3]] def odom_send(data): """ Returns the odometry of @@ -139,6 +177,8 @@ def odom_send(data): msg.twist.twist.angular.z = data[4].value[2] return msg +#digital_twin_jointstate_file_record = open("/home/fabrice/Documents/CRIStAL/prefab.MobileTrunk/test/join_states_data_processing/record_join_states/read_digital_twin_joint_states.txt", "w") +#summit_xl_jointstate_file_record = open('/home/fabrice/Documents/CRIStAL/prefab.MobileTrunk/test/join_states_data_processing/record_join_states/read_summit_xl_joint_states.txt', 'w') class SummitxlROSController(Sofa.Core.Controller): """A Simple keyboard controller for the SummitXL @@ -147,28 +187,20 @@ class SummitxlROSController(Sofa.Core.Controller): def __init__(self, *args, **kwargs): Sofa.Core.Controller.__init__(self, *args, **kwargs) self.robot = kwargs["robot"] - self.wheel_ray = 0.10 + self.robotToSim = kwargs["robotToSim"] self.flag = True - self.robot.robot_linear_x = 0 - self.robot.robot_angular_z = 0 self.time_now = None - - def move(self, fwd, angle): - """Move the robot using the forward speed and angular speed)""" - robot = RigidDof(self.robot.Chassis.position) - robot.translate(robot.forward * fwd) - robot.rotateAround([0, 1, 0], angle) - with self.robot.Chassis.WheelsMotors.angles.position.writeable() as angles: - #Make the wheel turn according to forward speed - # TODO: All the value are random, need to be really calculated - angles += (fwd/self.wheel_ray) - - #Make the wheel turn in reverse mode according to turning speed - # TODO: the value are random, need to be really calculated - angles[0] += (angle) - angles[2] += (angle) - angles[1] -= (angle) - angles[3] -= (angle) + self.time_s = time.time() + # self.positon_inital=[self.robot.Chassis.Base.position.position.value[0][0], + # self.robot.Chassis.Base.position.position.value[0][1], + # self.robot.Chassis.Base.position.position.value[0][2]] + self.t = 0 + self.sofa_time = 0 + self.robot_time = 0 + self.wheels_angular_speed = None + self.item = [0, 0, 0] + # self.angular_speed = 0 + # self.linear_speed = 0 def init_pose(self): """ @@ -177,8 +209,8 @@ def init_pose(self): """ if self.flag: - print("init summit_xl pose") - with self.robot.Chassis.position.position.writeable() as summit_pose: + + with self.robot.Chassis.Base.position.position.writeable() as summit_pose: #position x, y z summit_pose[0][0] = self.robot.reel_position[0] @@ -190,46 +222,87 @@ def init_pose(self): summit_pose[0][4] = self.robot.reel_orientation[1] summit_pose[0][5] = self.robot.reel_orientation[2] summit_pose[0][6] = self.robot.reel_orientation[3] + + self.t = 0 + self.sofa_time = 0 + self.robot_time = 0 self.flag = False + print("init summit_xl pose") def onAnimateBeginEvent(self, event): """ At each time step we move the robot by the given forward_speed and angular_speed) """ - # time init - if self.time_now is not None: - dt = float(self.robot.timestamp.value[0])+float(self.robot.timestamp.value[1])/1000000000 - self.time_now - self.time_now = float(self.robot.timestamp.value[0])+float(self.robot.timestamp.value[1])/1000000000 - else: - dt=0 - self.time_now = float(self.robot.timestamp.value[0])+float(self.robot.timestamp.value[1])/1000000000 - - self.robot.robot_linear_x = self.robot.robot_linear_vel[0] * dt - self.robot.robot_angular_z = self.robot.robot_angular_vel[2] * dt - - with self.robot.Chassis.Debug.position.position.writeable() as debug_pose: - for i in range(0, 3): - debug_pose[0][i] = self.robot.Chassis.position.position.value[0][i] - - for i in range(0, 4): - debug_pose[0][3+i] = self.robot.reel_orientation[i] - - with self.robot.Chassis.Reel_robot.position.position.writeable() as robot_pose: - for i in range(0,3): - robot_pose[0][i] = self.robot.reel_position[i] + dt = event['dt'] + if self.robotToSim: + if self.time_now is not None and not self.flag: + self.t = float(self.robot.timestamp.value[0])+float(self.robot.timestamp.value[1])/1000000000 - self.time_now + self.robot_time +=self.t + self.time_now = float(self.robot.timestamp.value[0])+float(self.robot.timestamp.value[1])/1000000000 + self.sofa_time +=dt + q.put([self.robot_time, self.robot.robot_angular_vel[2], self.robot.robot_linear_vel[0]]) + else: + self.t=0 + self.time_now = float(self.robot.timestamp.value[0])+float(self.robot.timestamp.value[1])/1000000000 + + if not self.robotToSim: + self.time_s = time.time() + if self.time_now is not None: + dt = self.time_s - self.time_now + self.time_now = time.time() + else: + dt = 0 + self.time_now = time.time() + robot_time = self.time_s + with self.robot.timestamp.writeable() as t: + t[0] = int(robot_time) + t[1] = 0 + self.wheels_angular_speed = twistToWheelsAngularSpeed(self.robot.robot_angular_vel[2], + self.robot.robot_linear_vel[0]) + move(self.robot.Chassis.WheelsMotors.angles.rest_position, self.wheels_angular_speed, dt) + + + # if self.robot.robot_angular_vel[2] ==0 and self.robot.robot_linear_vel[0] ==0 and self.robot_time > 10: + # summit_xl_jointstate_file_record.close() + # else: + # summit_xl_joint_state0 = json.dumps(self.robot.summit_xl_joints_states_vel[0]) + # summit_xl_joint_state1 = json.dumps(self.robot.summit_xl_joints_states_vel[1]) + # summit_xl_joint_state2 = json.dumps(self.robot.summit_xl_joints_states_vel[2]) + # summit_xl_joint_state3 = json.dumps(self.robot.summit_xl_joints_states_vel[3]) + # summit_xl_jointstate_file_record.write(str(self.robot_time) + " , " + str([summit_xl_joint_state0 , summit_xl_joint_state1, summit_xl_joint_state2, + # summit_xl_joint_state3]) + "\n") + if not self.flag and not q.empty(): + print(q.queue[0][0] - self.sofa_time) + if q.queue[0][0] - self.sofa_time <= 0.001: + # print("==== d queue ======") + # print(q.queue[0][0], " ", self.sofa_time, " " ,q.queue[0][0] - self.sofa_time) + # print("self.robot.robot_angular_vel[2] = ", q.queue[0][1], "self.robot.robot_linear_vel[0]= ", q.queue[0][2], " delta = ", + # q.queue[0][0]-self.item[0]) + self.item = q.get() + self.wheels_angular_speed = twistToWheelsAngularSpeed(self.item[1], + self.item[2]) + if(self.wheels_angular_speed is not None): + # print(" move : ", self.wheels_angular_speed , " dt = ", dt) + move(self.robot.Chassis.WheelsMotors.angles.rest_position, self.wheels_angular_speed, dt) for i in range(0,4): - robot_pose[0][3+i] = self.robot.reel_orientation[i] + self.robot.sim_orientation[i] = self.robot.Chassis.Base.position.position.value[0][3+i] + self.robot.digital_twin_joints_states_vel[i] = self.wheels_angular_speed[i] - for i in range(0,4): - self.robot.sim_orientation[i] = self.robot.Chassis.position.position.value[0][3+i] + for i in range(0,3): + self.robot.sim_position[i] = self.robot.Chassis.Base.position.position.value[0][i] - for i in range(0,3): - self.robot.sim_position[i] = self.robot.Chassis.position.position.value[0][i] + # digital_twin_joint_state0 = json.dumps(self.wheels_angular_speed[0]) + # digital_twin_joint_state1 = json.dumps(self.wheels_angular_speed[1]) + # digital_twin_joint_state2 = json.dumps(self.wheels_angular_speed[2]) + # digital_twin_joint_state3 = json.dumps(self.wheels_angular_speed[3]) - self.move(self.robot.robot_linear_x, self.robot.robot_angular_z) + # digital_twin_jointstate_file_record.write(str(self.sofa_time) + " , " + str([digital_twin_joint_state0, digital_twin_joint_state1 , digital_twin_joint_state2, + # digital_twin_joint_state3]) + "\n") + #if q.empty(): + # digital_twin_jointstate_file_record.close() # Wait to start receiving data from ROS to initialize the position # of the robot in the simulation with the position of the real robot if self.robot.reel_position[0] != 0: diff --git a/mobile_trunk_sim/wheels_angles_compute.py b/mobile_trunk_sim/wheels_angles_compute.py new file mode 100644 index 0000000..1a7cacd --- /dev/null +++ b/mobile_trunk_sim/wheels_angles_compute.py @@ -0,0 +1,98 @@ +from geometry_msgs.msg import Pose2D +import math + +#wheel_ray = 0.117 # rayon de la roue +#lz = 0.229 #distance entre le centre d'une des roues et le centre du chassis sur l'axe des z (458/2) +#lx = 0.2355 #distance entre le centre d'une des roues et le centre du chassis sur l'axe des x(142/2 + (614-(2*142))/2) + +wheel_base_ = 0.569 #distance between front and rear axles +track_width_ = 0.543 #distance between right and left wheels +wheel_diameter_ = 0.233 #wheel diamater, to convert from angular speed to linear +wheels_max_speed = 27.27 # rad/s +wheels_angular_speed = [0, 0, 0, 0] + +def twistToWheelsAngularSpeed(wy, vz): + """ + The robot moves along the z axis and rotates on the y axis. + twistToWheelsAngles method calculate the angular velocity + of each of the wheels + + """ + lab = wheel_base_ / 2.0 + track_width_ / 2.0 + + frw2 = (vz + lab * wy) / (wheel_diameter_ / 2.0) # roue avant droite + flw1 = (vz - lab * wy) / (wheel_diameter_ / 2.0) # roue avant gauche + blw3 = (vz - lab * wy) / (wheel_diameter_ / 2.0) # roue arriere gauche + brw4 = (vz + lab * wy) / (wheel_diameter_ / 2.0) #roue arrière droite + + wheels_angular_speed = [flw1 , frw2 , blw3, brw4] + + wheels_angular_speed = setJointVelocityReferenceBetweenLimits(wheels_angular_speed) + + return wheels_angular_speed + +def setJointVelocityReferenceBetweenLimits(wheels_angular_speed): + """ + + """ + max_scale_factor = 1.0 + lower_limit = - wheels_max_speed + upper_limit = wheels_max_speed + lower_scale_factor = upper_scale_factor = 1.0 + for i in range(0, 4): + + if wheels_angular_speed[i] < lower_limit: + lower_scale_factor = abs(wheels_angular_speed[i] / lower_limit) + + if wheels_angular_speed[i] < upper_limit: + upper_scale_factor = abs(wheels_angular_speed[i] / upper_limit) + + max_scale_factor = max(max_scale_factor, max(lower_scale_factor, upper_scale_factor)) + + for i in range(0, 4): + wheels_angular_speed[i] /= max_scale_factor + i = i+1 + return wheels_angular_speed + +def updateOdometry(dt): + vx = 0.0 + w = 0.0 + vz = 0.0 + robot_pose_ = Pose2D() + frw1 = wheels_angular_speed[0] + flw2 = wheels_angular_speed[1] + blw3 = wheels_angular_speed[2] + brw4 = wheels_angular_speed[3] + + if (abs(frw1) < 0.001) : frw1 = 0.0 + if (abs(flw2) < 0.001) : flw2 = 0.0 + if (abs(blw3) < 0.001) : blw3 = 0.0 + if (abs(brw4) < 0.001) : brw4 = 0.0 + + v_left_mps = ((flw2 + blw3) / 2.0) + v_right_mps = ((frw1 + brw4) / 2.0) + + fDistanceBetweenWheels = 0.92 + vz = (v_right_mps + v_left_mps) / 2.0 #m/s + vx = 0.0 + w = (v_right_mps - v_left_mps) / fDistanceBetweenWheels; # rad/s + robot_pose_.x += math.cos(robot_pose_.theta) * vz * dt- math.sin(robot_pose_.theta) * vx * dt + robot_pose_.y += math.sin(robot_pose_.theta) * vz * dt+ math.cos(robot_pose_.theta) * vx * dt + # qt = Quaternion() + # qt.quaternion_from_euler(0,0, robot_pose_.theta) + # vt =Vector3(robot_pose_.x , robot_pose_.y, 0) + return robot_pose_ + +def move(WheelsMotors_angles_rest_position, wheels_angular_speed, dt): + """ + Move method allow the robot to move thanks to the angular velocity of + each of the wheels + """ + with WheelsMotors_angles_rest_position.writeable() as angles: + #Make the wheel turn according to their angular speed + angles[0] += wheels_angular_speed[0] * dt + angles[1] += wheels_angular_speed[1] * dt + angles[2] += wheels_angular_speed[2] * dt + angles[3] += wheels_angular_speed[3] * dt + #print("------------>", WheelsMotors_angles_rest_position.value) + return WheelsMotors_angles_rest_position.value \ No newline at end of file diff --git a/mobile_trunk_ws/src/summitxl/package.xml b/mobile_trunk_ws/src/summit_xl/package.xml similarity index 96% rename from mobile_trunk_ws/src/summitxl/package.xml rename to mobile_trunk_ws/src/summit_xl/package.xml index e739223..68525d5 100644 --- a/mobile_trunk_ws/src/summitxl/package.xml +++ b/mobile_trunk_ws/src/summit_xl/package.xml @@ -1,7 +1,7 @@ - summitxl + summit_xl 0.0.0 ROS pkg to control summitxl sim fabrice diff --git a/mobile_trunk_sim/collision_test.py b/mobile_trunk_ws/src/summit_xl/resource/summit_xl similarity index 100% rename from mobile_trunk_sim/collision_test.py rename to mobile_trunk_ws/src/summit_xl/resource/summit_xl diff --git a/mobile_trunk_ws/src/summit_xl/setup.cfg b/mobile_trunk_ws/src/summit_xl/setup.cfg new file mode 100644 index 0000000..d579d06 --- /dev/null +++ b/mobile_trunk_ws/src/summit_xl/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/summit_xl +[install] +install_scripts=$base/lib/summit_xl diff --git a/mobile_trunk_ws/src/summitxl/setup.py b/mobile_trunk_ws/src/summit_xl/setup.py similarity index 79% rename from mobile_trunk_ws/src/summitxl/setup.py rename to mobile_trunk_ws/src/summit_xl/setup.py index 56f3df9..029acc6 100644 --- a/mobile_trunk_ws/src/summitxl/setup.py +++ b/mobile_trunk_ws/src/summit_xl/setup.py @@ -1,6 +1,6 @@ from setuptools import setup -package_name = 'summitxl' +package_name = 'summit_xl' setup( name=package_name, @@ -20,8 +20,7 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'summitxl_teleop_key = summitxl.summitxl_teleop_key:main', - 'summitxl_node = summitxl.summitxl_node:main' + 'summit_xl_teleop_key = summit_xl.summit_xl_teleop_key:main' ], }, ) diff --git a/mobile_trunk_ws/src/summitxl/summitxl/__init__.py b/mobile_trunk_ws/src/summit_xl/summit_xl/__init__.py similarity index 100% rename from mobile_trunk_ws/src/summitxl/summitxl/__init__.py rename to mobile_trunk_ws/src/summit_xl/summit_xl/__init__.py diff --git a/mobile_trunk_ws/src/summitxl/summitxl/summitxl_teleop_key.py b/mobile_trunk_ws/src/summit_xl/summit_xl/summit_xl_teleop_key.py similarity index 99% rename from mobile_trunk_ws/src/summitxl/summitxl/summitxl_teleop_key.py rename to mobile_trunk_ws/src/summit_xl/summit_xl/summit_xl_teleop_key.py index 68b5f75..1c404b1 100644 --- a/mobile_trunk_ws/src/summitxl/summitxl/summitxl_teleop_key.py +++ b/mobile_trunk_ws/src/summit_xl/summit_xl/summit_xl_teleop_key.py @@ -120,4 +120,4 @@ def main(): if __name__ == '__main__': - main() + main() \ No newline at end of file diff --git a/mobile_trunk_ws/src/summitxl/test/test_copyright.py b/mobile_trunk_ws/src/summit_xl/test/test_copyright.py similarity index 100% rename from mobile_trunk_ws/src/summitxl/test/test_copyright.py rename to mobile_trunk_ws/src/summit_xl/test/test_copyright.py diff --git a/mobile_trunk_ws/src/summitxl/test/test_flake8.py b/mobile_trunk_ws/src/summit_xl/test/test_flake8.py similarity index 100% rename from mobile_trunk_ws/src/summitxl/test/test_flake8.py rename to mobile_trunk_ws/src/summit_xl/test/test_flake8.py diff --git a/mobile_trunk_ws/src/summitxl/test/test_pep257.py b/mobile_trunk_ws/src/summit_xl/test/test_pep257.py similarity index 100% rename from mobile_trunk_ws/src/summitxl/test/test_pep257.py rename to mobile_trunk_ws/src/summit_xl/test/test_pep257.py diff --git a/mobile_trunk_ws/src/summitxl/resource/summitxl b/mobile_trunk_ws/src/summitxl/resource/summitxl deleted file mode 100644 index e69de29..0000000 diff --git a/mobile_trunk_ws/src/summitxl/setup.cfg b/mobile_trunk_ws/src/summitxl/setup.cfg deleted file mode 100644 index 6956fb8..0000000 --- a/mobile_trunk_ws/src/summitxl/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/summitxl -[install] -install_scripts=$base/lib/summitxl diff --git a/mobile_trunk_ws/src/summitxl/summitxl/summitxl_node.py b/mobile_trunk_ws/src/summitxl/summitxl/summitxl_node.py deleted file mode 100644 index 0ad6ad0..0000000 --- a/mobile_trunk_ws/src/summitxl/summitxl/summitxl_node.py +++ /dev/null @@ -1,44 +0,0 @@ -# coding: utf8 -#!/usr/bin/env python3 -import std_msgs -from std_msgs.msg import Float32MultiArray -import rclpy -from rclpy.node import Node -import sys - -class MinimalPublisher(Node): - - def __init__(self, arg): - super().__init__('summit_xl_node') - self.publisher_ = self.create_publisher(Float32MultiArray, '/summit_xl_control/cmd_vel', 10) - timer_period = 0.05 # seconds - self.timer = self.create_timer(timer_period, self.timer_callback) - self.linear_vel = arg[0] - self.angular_vel = arg[1] - - def timer_callback(self): - - msg = Float32MultiArray(layout=std_msgs.msg.MultiArrayLayout( data_offset=0), data=[self.linear_vel,0., - 0.,0.,0.,self.angular_vel]) - self.publisher_.publish(msg) - - -def main(args): - vel = [float(args[1]) ,float(args[2])] - rclpy.init() - - minimal_publisher = MinimalPublisher(vel) - - rclpy.spin(minimal_publisher) - - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - minimal_publisher.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - print(len(sys.argv)) - - main(sys.argv) \ No newline at end of file diff --git a/test/join_states_data_processing/digital_twin_data_interpolation.py b/test/join_states_data_processing/digital_twin_data_interpolation.py new file mode 100644 index 0000000..61563af --- /dev/null +++ b/test/join_states_data_processing/digital_twin_data_interpolation.py @@ -0,0 +1,65 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.interpolate import make_interp_spline +from scipy import signal + +# Lecture du fichier de données +with open("/home/fabrice/Documents/CRIStAL/prefab.MobileTrunk/test/join_states_data_processing/record_join_states/read_digital_twin_joint_states.txt", "r") as f: + lines = f.readlines() + +summit_xl_joint_state_wheels_dic= {"vitesse 2" :"digital_twin_front_right_wheel", + "vitesse 1" :" digital_twin_front_left_wheel ", + "vitesse 3" :" digital_twin_back_left_wheel ", + "vitesse 4" :"digital_twin_back_right_wheel "} + +temps = [] +vitesses = [[] for _ in range(4)] + +for line in lines: + elements = line.strip().split(',') + temps.append(float(elements[0])) + for i in range(4): + vitesses[i].append(float(elements[i+1].strip("[]' "))) + +# trier les valeurs de temps +sorted_temps = np.sort(temps) + + + +# Supprimer les doublons dans temps et extraire les indices uniques +unique_indices = np.unique(temps, return_index=True)[1] + +# Interpolation avec splines cubiques +xnew = np.linspace(min(temps), max(temps), num=500) +vitesses_smooth = [] +for i in range(4): + # Extraire les valeurs correspondantes dans vitesses[i] + unique_vitesses = np.array(vitesses[i])[unique_indices] + # Créer le spline avec les temps uniques et les vitesses correspondantes + spl = make_interp_spline(np.array(temps)[unique_indices], unique_vitesses, k=3) + vitesses_smooth.append(spl(xnew)) + +# Filtrage passe-bas avec la fréquence de coupure modifiée +fc = 0.3 # fréquence de coupure initiale +n = len(xnew) # nombre total de points de données +duree = xnew[-1] - xnew[0] # durée totale de l'enregistrement +fs = n / duree # fréquence d'échantillonnage +fc_modifiee = 1 # nouvelle fréquence de coupure +w = fc_modifiee / (fs / 2) # normalisation de la nouvelle fréquence de coupure +b, a = signal.butter(1, w, 'lowpass') +vitesses_filtre = [] +for i in range(4): + vitesses_filtre.append(signal.filtfilt(b, a, vitesses_smooth[i])) + +# # Plot des données brutes et des courbes interpolées +# for i in range(4): +# fig, ax = plt.subplots() +# ax.plot(temps, vitesses[i], label=f"Vitesse {i+1} - brutes") +# #ax.plot(xnew, vitesses_smooth[i], label=f"Vitesse {i+1} - interp") +# ax.plot(xnew, vitesses_filtre[i], label=f"Vitesse {i+1} - filtre") +# ax.set_xlabel("Temps (s)") +# ax.set_ylabel(f"Vitesse de rotation(rad/s)") +# ax.text(0,4,"pour une vitesse de 0.3 m/s et une vitesse de rotation de 90°") +# ax.legend() +# ax.grid() +# plt.show() diff --git a/test/join_states_data_processing/join_states_data_processing.py b/test/join_states_data_processing/join_states_data_processing.py new file mode 100644 index 0000000..b2a4379 --- /dev/null +++ b/test/join_states_data_processing/join_states_data_processing.py @@ -0,0 +1,108 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.interpolate import make_interp_spline +from scipy import signal + +with open("/home/fabrice/Documents/CRIStAL/prefab.MobileTrunk/test/join_states_data_processing/record_join_states/read_digital_twin_joint_states_0.5_m_par_s.txt", "r") as f: + digital_twin_lines = f.readlines() + + +with open("/home/fabrice/Documents/CRIStAL/prefab.MobileTrunk/test/join_states_data_processing/record_join_states/read_summit_xl_joint_states_0.5_m_par_s.txt", "r") as f: + summit_xl_lines = f.readlines() + +summit_xl_dt = [] +summit_xl_speed = [[] for _ in range(4)] + +digital_twin_dt = [] +digital_twin_speed= [[] for _ in range(4)] + + + + +for line in summit_xl_lines: + elements = line.strip().split(',') + summit_xl_dt.append(float(elements[0])) + for i in range(4): + summit_xl_speed[i].append(float(elements[i+1].strip("[]' "))) + +for line in digital_twin_lines: + elements = line.strip().split(',') + digital_twin_dt.append(float(elements[0])) + for i in range(4): + digital_twin_speed[i].append(float(elements[i+1].strip("[]' "))) + + + +# # trier les valeurs de temps du jumeau numérique +# digital_twin_sorted_dt = np.sort(digital_twin_dt) +# # trier les valeurs de temps robot réel +# summit_xl_sorted_dt = np.sort(summit_xl_dt) + +# # Supprimer les doublons dans temps et extraire les indices uniques +# digital_twin_unique_indices = np.unique(digital_twin_dt, return_index=True)[1] +# summit_xl_unique_indices = np.unique(summit_xl_dt, return_index=True)[1] + + +# # Interpolation avec splines cubiques + +# # Pour le jumeau numérique +# digital_twin_xnew = np.linspace(min(digital_twin_dt), max(digital_twin_dt), num=500) +# digital_twin_smooth_speed = [] +# for i in range(4): +# # Extraire les valeurs correspondantes dans vitesses[i] +# digital_twin_unique_speed = np.array(digital_twin_speed[i])[digital_twin_unique_indices] +# # Créer le spline avec les temps uniques et les vitesses correspondantes +# spl = make_interp_spline(np.array(digital_twin_dt)[digital_twin_unique_indices], digital_twin_unique_speed, k=3) +# digital_twin_smooth_speed.append(spl(digital_twin_xnew)) + + +# # Pour le robot réel +# summit_xl_xnew = np.linspace(min(summit_xl_dt), max(summit_xl_dt), num=500) +# summit_xl_smooth_speed = [] +# for i in range(4): +# # Extraire les valeurs correspondantes dans vitesses[i] +# summit_xl_unique_speed = np.array(summit_xl_speed[i])[summit_xl_unique_indices] +# # Créer le spline avec les temps uniques et les vitesses correspondantes +# spl = make_interp_spline(np.array(summit_xl_dt)[summit_xl_unique_indices], summit_xl_unique_speed, k=3) +# summit_xl_smooth_speed.append(spl(summit_xl_xnew)) + + +# #Pour le jumeau numérique +# # Filtrage passe-bas avec la fréquence de coupure modifiée +# fc = 0.3 # fréquence de coupure initiale +# n = len(digital_twin_xnew) # nombre total de points de données +# duree = digital_twin_xnew[-1] - digital_twin_xnew[0] # durée totale de l'enregistrement +# fs = n / duree # fréquence d'échantillonnage +# fc_modifiee = 1# nouvelle fréquence de coupure +# w = fc_modifiee / (fs / 2) # normalisation de la nouvelle fréquence de coupure +# b, a = signal.butter(1, w, 'lowpass') +# digital_twin_xnew_filtred_speed = [] +# for i in range(4): +# digital_twin_xnew_filtred_speed.append(signal.filtfilt(b, a, digital_twin_smooth_speed[i])) + +# #Pour le summit_xl +# #Pour le jumeau numérique +# # Filtrage passe-bas avec la fréquence de coupure modifiée +# fc = 1 # fréquence de coupure initiale +# n = len(summit_xl_xnew) # nombre total de points de données +# duree = summit_xl_xnew[-1] - summit_xl_xnew[0] # durée totale de l'enregistrement +# fs = n / duree # fréquence d'échantillonnage +# fc_modifiee = 1 # nouvelle fréquence de coupure +# w = fc_modifiee/ (fs / 2) # normalisation de la nouvelle fréquence de coupure +# b, a = signal.butter(1, w, 'lowpass') +# summit_xl_xnew_filtred_speed = [] +# for i in range(4): +# summit_xl_xnew_filtred_speed.append(signal.filtfilt(b, a, summit_xl_smooth_speed[i])) + +# Plot des données +for i in range(4): + fig, ax = plt.subplots() + ax.plot(digital_twin_dt, digital_twin_speed[i], label=f"Vitesse {i+1}") + #ax.plot(digital_twin_xnew , digital_twin_xnew_filtred_speed[i], label=f"digital_twin angular_speed{i+1} - filtre") + ax.plot(summit_xl_dt , summit_xl_speed[i], label=f"Summit_xl angular_speed{i+1}") + ax.set_xlabel("Temps (s)") + ax.set_ylabel(f"Vitesse de rotation(rad/s)") + #ax.text(0,4,"pour une vitesse de 0.3 m/s et une vitesse de rotation de 90°") + ax.legend() + ax.grid() +plt.show() diff --git a/test/join_states_data_processing/summit_xl_data_interpolation.py b/test/join_states_data_processing/summit_xl_data_interpolation.py new file mode 100644 index 0000000..b8e0360 --- /dev/null +++ b/test/join_states_data_processing/summit_xl_data_interpolation.py @@ -0,0 +1,64 @@ +import numpy as np +import matplotlib.pyplot as plt +from scipy.interpolate import make_interp_spline +from scipy import signal + +# Lecture du fichier de données +with open("/home/fabrice/Documents/CRIStAL/prefab.MobileTrunk/test/join_states_data_processing/record_join_states/read_summit_xl_joint_states.txt", "r") as f: + lines = f.readlines() + +summit_xl_joint_state_wheels_dic = { + "vitesse 2": "digital_twin_front_right_wheel", + "vitesse 1": " digital_twin_front_left_wheel", + "vitesse 3": " digital_twin_back_left_wheel", + "vitesse 4": "digital_twin_back_right_wheel" +} + +temps = [] +vitesses = [[] for _ in range(4)] + +for line in lines: + elements = line.strip().split(',') + temps.append(float(elements[0])) + for i in range(4): + vitesses[i].append(float(elements[i+1].strip("[]' "))) + + +# # trier les valeurs de temps +# sorted_temps = np.sort(temps) + +# # Supprimer les doublons dans temps et extraire les indices uniques +# unique_indices = np.unique(temps, return_index=True)[1] + +# # Interpolation avec splines cubiques +# xnew = np.linspace(min(temps), max(temps), num=500) +# vitesses_smooth = [] +# for i in range(4): +# # Extraire les valeurs correspondantes dans vitesses[i] +# unique_vitesses = np.array(vitesses[i])[unique_indices] +# # Créer le spline avec les temps uniques et les vitesses correspondantes +# spl = make_interp_spline(np.array(temps)[unique_indices], unique_vitesses, k=3) +# vitesses_smooth.append(spl(xnew)) + +# # Filtrage passe-bas avec la fréquence de coupure modifiée +# fc = 0.3 # fréquence de coupure initiale +# n = len(xnew) # nombre total de points de données +# duree = xnew[-1] - xnew[0] # durée totale de l'enregistrement +# fs = n / duree # fréquence d'échantillonnage +# fc_modifiee = 1 # nouvelle fréquence de coupure +# w = fc_modifiee / (fs / 2) # normalisation de la nouvelle fréquence de coupure +# b, a = signal.butter(1, w, 'lowpass') +# vitesses_filtre = [] +# for i in range(4): +# vitesses_filtre.append(signal.filtfilt(b, a, vitesses_smooth[i])) + +# Plot des données brutes et des courbes interpolées +for i in range(4): + fig, ax = plt.subplots() + ax.plot(temps, vitesses[i] ,label=f"Vitesse {i+1} - brutes") + #ax.plot(xnew, vitesses_smooth[i], label=f"Vitesse {i+1} - interp") + #ax.plot(xnew, vitesses_filtre[i], label=f"Vitesse {i+1} - filtre") + ax.set_xlabel("Temps (s)") + ax.set_ylabel(f"Vitesse de rotation(rad/s)") + ax.text(0,4,"pour une vitesse de 0.3 m/s et une vitesse 90°") +plt.show() diff --git a/test/ros/launch/node_launch.launch b/test/ros/launch/node_launch.launch new file mode 100644 index 0000000..a90632b --- /dev/null +++ b/test/ros/launch/node_launch.launch @@ -0,0 +1,36 @@ +# File: node_launch.yaml +launch: +# args +- arg: + name: "vitesse_angulaire" + default: "0.3" +- arg: + name: "vitesse_lineaire" + default: "0.3" + + +# Nodes +- node: + pkg: "somorob" + exec: "summit_xl" + name: "summit_xl" + param: + - + name: "vitesse_angulaire" + value: "$(var vitesse_angulaire)" + - + name: "vitesse_lineaire" + value: "$(var vitesse_lineaire)" + + +- node: + pkg: "bag_recorder_nodes" + exec: "bag_recorder" + name: "bag_recorder" + param: + - + name: "vitesse_angulaire" + value: "$(var vitesse_angulaire)" + - + name: "vitesse_lineaire" + value: "$(var vitesse_lineaire)" diff --git a/test/ros/src/bag_recorder_nodes/CMakeLists.txt b/test/ros/src/bag_recorder_nodes/CMakeLists.txt new file mode 100644 index 0000000..fe0d220 --- /dev/null +++ b/test/ros/src/bag_recorder_nodes/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.5) +project(bag_recorder_nodes) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rosbag2_cpp REQUIRED) +find_package(rosidl_default_generators REQUIRED) + + +add_executable(bag_recorder src/bag_recorder.cpp) + +ament_target_dependencies(bag_recorder rclcpp rosbag2_cpp rosidl_typesupport_cpp geometry_msgs nav_msgs sensor_msgs) + +install(TARGETS + bag_recorder + DESTINATION lib/${PROJECT_NAME} +) + + +ament_package() diff --git a/test/ros/src/bag_recorder_nodes/package.xml b/test/ros/src/bag_recorder_nodes/package.xml new file mode 100644 index 0000000..099a12c --- /dev/null +++ b/test/ros/src/bag_recorder_nodes/package.xml @@ -0,0 +1,21 @@ + + + + bag_recorder_nodes + 0.0.0 + TODO: Package description + fabrice + TODO: License declaration + + ament_cmake + + rclcpp + rosbag2_cpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/test/ros/src/bag_recorder_nodes/src/bag_recorder.cpp b/test/ros/src/bag_recorder_nodes/src/bag_recorder.cpp new file mode 100644 index 0000000..ded6085 --- /dev/null +++ b/test/ros/src/bag_recorder_nodes/src/bag_recorder.cpp @@ -0,0 +1,159 @@ +#include +#include +#include + +#include +#include +#include +#include +#include + +using std::placeholders::_1; + +class SimpleBagRecorder : public rclcpp::Node +{ +public: + SimpleBagRecorder(double vitesse_angulaire=-10., double vitesse_lineaire=10.) : Node("simple_bag_recorder"), + m_vitesse_angulaire(vitesse_angulaire), m_vitesse_lineaire(vitesse_lineaire) + { + // Get parameters from launch file + vitesse_angulaire = this->declare_parameter("vitesse_angulaire", 0.0); + vitesse_lineaire = this->declare_parameter("vitesse_lineaire", 0.0); + + m_vitesse_angulaire = this->get_parameter("vitesse_angulaire").get_value(); + m_vitesse_lineaire = this->get_parameter("vitesse_lineaire").get_value(); + + // Create base directory if it doesn't exist + if(!std::filesystem::exists("record_folder")) + { + std::filesystem::create_directory("record_folder"); + } + + // If the folder already exist move to the base directory + std::filesystem::current_path("record_folder"); + + //Check if the velocities are different from zero bzfor creating the folder + if(m_vitesse_angulaire !=-10. && m_vitesse_lineaire !=-10.) + { + std::string base_folder_name = "w_" + std::to_string(m_vitesse_angulaire) + "_v_"+ + std::to_string(m_vitesse_lineaire); + + //create the new folder withthe velocities as forder name + if(!std::filesystem::exists(base_folder_name)) + { + std::filesystem::create_directory(base_folder_name); + + } + else + { + std::filesystem::remove_all(base_folder_name); + std::filesystem::create_directory(base_folder_name); + + } + + const rosbag2_cpp::StorageOptions storage_options({base_folder_name, "sqlite3"}); + + const rosbag2_cpp::ConverterOptions converter_options( + {rmw_get_serialization_format(), + rmw_get_serialization_format()}); + + writer_ = std::make_unique(); + + writer_->open(storage_options, converter_options); + + writer_->create_topic( + {"/summit_xl/robotnik_base_control/odom", + "nav_msgs/msg/Odometry", + rmw_get_serialization_format(), + ""}); + + writer_->create_topic( + {"/summit_xl/joint_states", + "sensor_msgs/msg/JointState", + rmw_get_serialization_format(), + ""}); + + subscription_odom = create_subscription( + "/summit_xl/robotnik_base_control/odom", 10, std::bind(&SimpleBagRecorder::odom_topic_callback, this, _1)); + + subscription_join_state = create_subscription( + "/summit_xl/joint_states", 10, std::bind(&SimpleBagRecorder::join_state_topic_callback, this, _1)); + } + else{ + RCLCPP_ERROR(get_logger(), "Invalid velocities"); + } + + } +private: + void odom_topic_callback(std::shared_ptr msg) const + { + auto bag_message = std::make_shared(); + + bag_message->serialized_data = std::shared_ptr( + new rcutils_uint8_array_t, + [this](rcutils_uint8_array_t *msg) { + auto fini_return = rcutils_uint8_array_fini(msg); + delete msg; + if (fini_return != RCUTILS_RET_OK) { + RCLCPP_ERROR(get_logger(), + "Failed to destroy serialized message %s", rcutils_get_error_string().str); + } + }); + + *bag_message->serialized_data = msg->release_rcl_serialized_message(); + //Convert the received message to a serialized message and store it in the bag message + //rosbag2_cpp::typesupport_helpers::to_storage_format( + //*msg, rmw_get_serialization_format(), *bag_message->serialized_data); + + bag_message->topic_name = "/summit_xl/robotnik_base_control/odom"; + if (rcutils_system_time_now(&bag_message->time_stamp) != RCUTILS_RET_OK) { + RCLCPP_ERROR(get_logger(), "Error getting current time: %s", + rcutils_get_error_string().str); + } + + writer_->write(bag_message); + } + + void join_state_topic_callback(std::shared_ptr msg) const + { + auto bag_message = std::make_shared(); + + bag_message->serialized_data = std::shared_ptr( + new rcutils_uint8_array_t, + [this](rcutils_uint8_array_t *msg) { + auto fini_return = rcutils_uint8_array_fini(msg); + delete msg; + if (fini_return != RCUTILS_RET_OK) { + RCLCPP_ERROR(get_logger(), + "Failed to destroy serialized message %s", rcutils_get_error_string().str); + } + }); + + *bag_message->serialized_data = msg->release_rcl_serialized_message(); + //Convert the received message to a serialized message and store it in the bag message + //rosbag2_cpp::typesupport_helpers::to_storage_format( + //*msg, rmw_get_serialization_format(), *bag_message->serialized_data); + + bag_message->topic_name = "/summit_xl/joint_states"; + if (rcutils_system_time_now(&bag_message->time_stamp) != RCUTILS_RET_OK) { + RCLCPP_ERROR(get_logger(), "Error getting current time: %s", + rcutils_get_error_string().str); + } + + writer_->write(bag_message); + } + + rclcpp::Subscription::SharedPtr subscription_odom; + rclcpp::Subscription::SharedPtr subscription_join_state; + std::unique_ptr writer_; + double m_vitesse_angulaire, m_vitesse_lineaire; +}; + + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; + } diff --git a/test/ros/src/somorob/CMakeLists.txt b/test/ros/src/somorob/CMakeLists.txt new file mode 100644 index 0000000..d70d5f6 --- /dev/null +++ b/test/ros/src/somorob/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.5) +project(somorob) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) + +add_executable(summit_xl src/summit_xl.cpp) + +target_include_directories(summit_xl PRIVATE ${geometry_msgs_INCLUDE_DIRS}) + +ament_target_dependencies(summit_xl rclcpp geometry_msgs) + + +install(TARGETS + summit_xl + DESTINATION lib/${PROJECT_NAME}) + + +ament_package() diff --git a/test/ros/src/somorob/package.xml b/test/ros/src/somorob/package.xml new file mode 100644 index 0000000..7529e22 --- /dev/null +++ b/test/ros/src/somorob/package.xml @@ -0,0 +1,22 @@ + + + + somorob + 0.0.0 + TODO: Package description + fabrice + TODO: License declaration + + ament_cmake + + std_msgs + rclcpp + geometry_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/test/ros/src/somorob/src/summit_xl.cpp b/test/ros/src/somorob/src/summit_xl.cpp new file mode 100644 index 0000000..84b26d1 --- /dev/null +++ b/test/ros/src/somorob/src/summit_xl.cpp @@ -0,0 +1,113 @@ +#include +#include +#include +#include + +using namespace std::chrono_literals; + +class TeleopTwistTest : public rclcpp::Node +{ +public: + TeleopTwistTest(double vitesse_angulaire = 0, double vitesse_lineaire = 0) + : Node("teleop_twist_test"), m_vitesse_angulaire(vitesse_angulaire), m_vitesse_lineaire(vitesse_lineaire) + { + publisher_ = this->create_publisher("/summit_xl/robotnik_base_control/cmd_vel", 100); + timer_ = this->create_wall_timer(10ms, std::bind(&TeleopTwistTest::timer_callback, this)); + vitesse_angulaire = this->declare_parameter("vitesse_angulaire", 0.0); + vitesse_lineaire = this->declare_parameter("vitesse_lineaire", 0.0); + + m_vitesse_lineaire = this->get_parameter("vitesse_lineaire").get_value(); + m_vitesse_angulaire = this->get_parameter("vitesse_angulaire").get_value(); + } + +private: + void timer_callback() + { + time_now_ = this->get_clock()->now(); + + auto dt_duration = time_now_ - time_s_; + double dt = dt_duration.seconds(); + + if (dt > 0.0 && dt < 10.0) + { + // RCLCPP_INFO(this->get_logger(), "etape 1 = %f", dt); + geometry_msgs::msg::Twist twist; + twist.linear.x = m_vitesse_lineaire; twist.linear.y = 0.0; twist.linear.z = 0.0; + twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.; + publisher_->publish(twist); + } + + if (dt > 10.0 && dt < 20.0) + { + // RCLCPP_INFO(this->get_logger(), "etape 2 = %f", dt); + geometry_msgs::msg::Twist twist; + twist.linear.x = 0.; twist.linear.y = 0.0; twist.linear.z = 0.0; + twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = m_vitesse_angulaire; + publisher_->publish(twist); + } + + if (dt > 20.0 && dt < 30.0) + { + // RCLCPP_INFO(this->get_logger(), "etape 3 = %f", dt); + geometry_msgs::msg::Twist twist; + twist.linear.x = m_vitesse_lineaire; twist.linear.y = 0.0; twist.linear.z = 0.0; + twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.; + publisher_->publish(twist); + } + + if (dt > 30.0 && dt < 40.0) + { + // RCLCPP_INFO(this->get_logger(), "etape 4 = %f", dt); + geometry_msgs::msg::Twist twist; + twist.linear.x = 0.; twist.linear.y = 0.0; twist.linear.z = 0.0; + twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = m_vitesse_angulaire; + publisher_->publish(twist); + } + + if (dt > 40.0 && dt < 50.0) + { + // RCLCPP_INFO(this->get_logger(), "etape 5 = %f", dt); + geometry_msgs::msg::Twist twist; + twist.linear.x = m_vitesse_lineaire; twist.linear.y = 0.0; twist.linear.z = 0.0; + twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.; + publisher_->publish(twist); + } + + if (dt > 50.0 && dt < 60.0) + { + // RCLCPP_INFO(this->get_logger(), "etape 6 = %f", dt); + geometry_msgs::msg::Twist twist; + twist.linear.x = 0.; twist.linear.y = 0.0; twist.linear.z = 0.0; + twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = m_vitesse_angulaire; + publisher_->publish(twist); + } + + if (dt > 60.0 && dt < 70.0) + { + // RCLCPP_INFO(this->get_logger(), "etape 7 = %f", dt); + geometry_msgs::msg::Twist twist; + twist.linear.x = m_vitesse_lineaire; twist.linear.y = 0.0; twist.linear.z = 0.0; + twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.; + publisher_->publish(twist); + } + if (dt > 70.0) + { + RCLCPP_INFO(this->get_logger(), "Job done = %f", dt); + } + } + + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Time time_now_; + rclcpp::Time time_s_ = this->get_clock()->now(); + double m_vitesse_angulaire, m_vitesse_lineaire; +}; + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file