diff --git a/mobile_trunk_sim/echelon3/BaseController.py b/mobile_trunk_sim/echelon3/BaseController.py deleted file mode 100644 index 8190c52..0000000 --- a/mobile_trunk_sim/echelon3/BaseController.py +++ /dev/null @@ -1,38 +0,0 @@ -import Sofa -import numpy as np - - -class BaseController(Sofa.Core.Controller): - """ - Controller to control the base in forward kinematics - Parameters : - mechanicalObject : the mechanicalObject to control - name : the name of the component - """ - - def __init__(self, mechanicalObject : Sofa.Core.Node, maxDisplacement : float, name : str = "BaseController"): - Sofa.Core.Controller.__init__(self, name = name) - - self.mechanicalObject = mechanicalObject - self.maxDisplacement = maxDisplacement - self.displacement = 0 - - - # #action when key press - def onKeypressedEvent(self, c): - key = c['key'] - base = np.array(self.mechanicalObject.rest_position.value) - print(base) - - if key == "+": - if self.displacement <= self.maxDisplacement: - base[0][0] +=1 - self.mechanicalObject.rest_position.value = base.tolist() - self.displacement+=1 - - if key == "-": - if self.displacement >= -self.maxDisplacement: - base[0][0] -=1 - self.mechanicalObject.rest_position.value = base.tolist() - self.displacement-=1 - diff --git a/mobile_trunk_sim/echelon3/CableController.py b/mobile_trunk_sim/echelon3/CableController.py index 4ff5c98..17aa9a9 100644 --- a/mobile_trunk_sim/echelon3/CableController.py +++ b/mobile_trunk_sim/echelon3/CableController.py @@ -1,153 +1,55 @@ import Sofa - +import math class CableController(Sofa.Core.Controller): """ Controller to control the cables in forward kinematics Parameters : - mechanicalObject : the mechanicalObject to control + cables : list of the cables (constraint object) name : the name of the component """ - def __init__(self, mechanicalObject : Sofa.Core.Node, name : str = "CableController"): + def __init__(self, cables : list, name : str = "CableController"): Sofa.Core.Controller.__init__(self, name = name) - self.Cables = mechanicalObject - + 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'] - ##### Section 1 ###### - - # Cable 1 - if (key == "1"): - displacement = self.Cables.Section1Cable1.value.value+1 - if displacement[0] <= 20 : - self.Cables.Section1Cable1.value.value = [displacement] - print("length Section 1 Cable 1 : "+str(displacement[0])) - - - if (key == "4"): - displacement = self.Cables.Section1Cable1.value.value-1 - if displacement[0] >= -20 : - self.Cables.Section1Cable1.value.value = [displacement] - print("length Section 1 Cable 1 : "+str(displacement[0])) - - # Cable 2 - if (key == "2"): - displacement = self.Cables.Section1Cable2.value.value+1 - if displacement[0] <= 20 : - self.Cables.Section1Cable2.value.value = [displacement] - print("length Section 1 Cable 2 : "+str(displacement[0])) - - if (key == "5"): - displacement = self.Cables.Section1Cable2.value.value-1 - if displacement[0] >= -20 : - self.Cables.Section1Cable2.value.value = [displacement] - print("length Section 1 Cable 2 : "+str(displacement[0])) - - # cable 3 - if (key == "3"): - displacement = self.Cables.Section1Cable3.value.value+1 - if displacement[0] <= 20 : - self.Cables.Section1Cable3.value.value = [displacement] - print("length Section 1 Cable 3 : "+str(displacement[0])) - - if (key == "6"): - displacement = self.Cables.Section1Cable3.value.value-1 - if displacement[0] >= -20 : - self.Cables.Section1Cable3.value.value = [displacement] - print("length Section 1 Cable 3 : "+str(displacement[0])) - - - # ##### Section 2 ###### - - # # Cable 1 - # if (key == "A"): - # displacement = self.Cables.Section2Cable1.value.value+1 - # if displacement[0] <= 20 : - # self.Cables.Section2Cable1.value.value = [displacement] - # print("length Section 2 Cable 1 : "+str(displacement[0])) - - - # if (key == "B"): - # displacement = self.Cables.Section2Cable1.value.value-1 - # if displacement[0] >= -20 : - # self.Cables.Section2Cable1.value.value = [displacement] - # print("length Section 2 Cable 1 : "+str(displacement[0])) - - # # Cable 2 - # if (key == "C"): - # displacement = self.Cables.Section2Cable2.value.value+1 - # if displacement[0] <= 20 : - # self.Cables.Section2Cable2.value.value = [displacement] - # print("length Section 2 Cable 2 : "+str(displacement[0])) + key = c['key'] + displacement = [math.inf] - # if (key == "D"): - # displacement = self.Cables.Section2Cable2.value.value-1 - # if displacement[0] >= -20 : - # self.Cables.Section2Cable2.value.value = [displacement] - # print("length Section 2 Cable 2 : "+str(displacement[0])) - - # # cable 3 - # if (key == "E"): - # displacement = self.Cables.Section2Cable3.value.value+1 - # if displacement[0] <= 20 : - # self.Cables.Section2Cable3.value.value = [displacement] - # print("length Section 2 Cable 3 : "+str(displacement[0])) - - # if (key == "F"): - # displacement = self.Cables.Section2Cable3.value.value-1 - # if displacement[0] >= -20 : - # self.Cables.Section2Cable3.value.value = [displacement] - # print("length Section 2 Cable 3 : "+str(displacement[0])) - - # ##### Section 3 ###### - - # # Cable 1 - # if (key == "G"): - # displacement = self.Cables.Section3Cable1.value.value+1 - # if displacement[0] <= 20 : - # self.Cables.Section3Cable1.value.value = [displacement] - # print("length Section 3 Cable 1 : "+str(displacement[0])) - - - # if (key == "H"): - # displacement = self.Cables.Section3Cable1.value.value-1 - # if displacement[0] >= -20 : - # self.Cables.Section3Cable1.value.value = [displacement] - # print("length Section 3 Cable 1 : "+str(displacement[0])) - - # # Cable 2 - # if (key == "I"): - # displacement = self.Cables.Section3Cable2.value.value+1 - # if displacement[0] <= 20 : - # self.Cables.Section3Cable2.value.value = [displacement] - # print("length Section 3 Cable 2 : "+str(displacement[0])) - - # if (key == "J"): - # displacement = self.Cables.Section3Cable2.value.value-1 - # if displacement[0] >= -20 : - # self.Cables.Section3Cable2.value.value = [displacement] - # print("length Section 3 Cable 2 : "+str(displacement[0])) - - # # cable 3 - # if (key == "K"): - # displacement = self.Cables.Section3Cable3.value.value+1 - # if displacement[0] <= 20 : - # self.Cables.Section3Cable3.value.value = [displacement] - # print("length Section 3 Cable 3 : "+str(displacement[0])) - - # if (key == "L"): - # displacement = self.Cables.Section3Cable3.value.value-1 - # if displacement[0] >= -20 : - # self.Cables.Section3Cable3.value.value = [displacement] - # print("length Section 3 Cable 3 : "+str(displacement[0])) - - # action at end frame - def onAnimateEndEvent(self, eventType): - pass \ No newline at end of file + ##### 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 index b35c43a..a5984d1 100644 --- a/mobile_trunk_sim/echelon3/ForceController.py +++ b/mobile_trunk_sim/echelon3/ForceController.py @@ -10,11 +10,11 @@ class ForceController(Sofa.Core.Controller): name : the name of the component """ - def __init__(self, mechanicalObject : Sofa.Core.Node, dt : float, name : str = "CableController"): + def __init__(self, cables : list, dt : float, name : str = "CableController"): Sofa.Core.Controller.__init__(self, name = name) - self.Cables = mechanicalObject - self.forceMax = 49050000 # max 6kg + self.cables = cables + self.forceMax = 1e20 # max 6kg self.forceMin = 0 self.dt = dt @@ -25,43 +25,37 @@ def onAnimateBeginEvent(self, eventType): #action when key press def onKeypressedEvent(self, c): key = c['key'] - ##### Section 1 ###### - - # Cable 1 - if (key == "1"): - force = self.Cables.Section1Cable1.value.value + 981000 # F = g.mm.s-2 ==> 100g : 100*9810 = g.mm.s-2 - if force[0] <= self.forceMax : - self.Cables.Section1Cable1.value.value = [force*self.dt] - print("force Section 1 Cable 1 : "+str(force[0]*10**-6.0)) - - if (key == "4"): - force = self.Cables.Section1Cable1.value.value - 981000 # F = g.mm.s-2 ==> 500g : 500*9810 = 4.905*10^6 g.mm.s-2 - if force[0] >= self.forceMin : - self.Cables.Section1Cable1.value.value = [force * self.dt] - print("force Section 1 Cable 1 : "+str(force[0])) - - # Cable 2 - if (key == "2"): - force = self.Cables.Section1Cable2.value.value + 981000 # F = g.mm.s-2 ==> 500g : 500*9810 = 4.905*10^6 g.mm.s-2 - if force[0] <= self.forceMax : - self.Cables.Section1Cable2.value.value = [force * self.dt] - print("force Section 1 Cable 2 : "+str(force[0])) - - if (key == "5"): - force = self.Cables.Section1Cable2.value.value - 981000 # F = g.mm.s-2 ==> 500g : 500*9810 = 4.905*10^6 g.mm.s-2 - if force[0] >= self.forceMin : - self.Cables.Section1Cable2.value.value = [force * self.dt] - print("force Section 1 Cable 2 : "+str(force[0])) - # cable 3 - if (key == "3"): - force = self.Cables.Section1Cable3.value.value + 981000 # F = g.mm.s-2 ==> 500g : 500*9810 = 4.905*10^6 g.mm.s-2 + ##### 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.Section1Cable3.value.value = [force * self.dt] - print("force Section 1 Cable 3 : "+str(force[0])) - - if (key == "6"): - force = self.Cables.Section1Cable3.value.value - 981000 # F = g.mm.s-2 ==> 500g : 500*9810 = 4.905*10^6 g.mm.s-2 - if force[0] >= self.forceMin : - self.Cables.Section1Cable3.value.value = [force * self.dt] - print("force Section 1 Cable 3 : "+str(force[0])) + 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/SpringConnection.py b/mobile_trunk_sim/echelon3/SpringConnection.py deleted file mode 100644 index 400f202..0000000 --- a/mobile_trunk_sim/echelon3/SpringConnection.py +++ /dev/null @@ -1,68 +0,0 @@ - -from parameters import * -from classEchelon import * - -if base_translation : - from BaseController import * -if not inverse : - from CableController import * - - - -def createEchelon(echelon,base,index,translation,rotation): - - 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('EulerImplicitSolver', rayleighStiffness=parameters.numerical.rayleighStiffness, rayleighMass=parameters.numerical.rayleighMass, vdamping=parameters.numerical.vdamping) - # framesNode.addObject('EigenSimplicialLDLT', name='ldl', template='CompressedRowSparseMatrixMat3x3d'); - framesNode.addObject('SparseLDLSolver', name='ldl', template='CompressedRowSparseMatrixMat3x3d'); - framesNode.addObject('GenericConstraintCorrection' , solverName='ldl') - framesNode.addObject('PointSetTopologyContainer', position=position.output_position); - frames = framesNode.addObject('MechanicalObject', name='frames', template='Rigid3d', showObject=1, showObjectScale=0.01, 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=1e10 , angularStiffness=1e14) - - ######################################### - # 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() - - 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) - - - return parameters \ No newline at end of file diff --git a/mobile_trunk_sim/echelon3/classEchelon.py b/mobile_trunk_sim/echelon3/classEchelon.py index 96b1883..2377a07 100644 --- a/mobile_trunk_sim/echelon3/classEchelon.py +++ b/mobile_trunk_sim/echelon3/classEchelon.py @@ -4,7 +4,6 @@ from math import sin,cos,pi import numpy as np - @dataclass class Numerical: """ diff --git a/mobile_trunk_sim/echelon3/createEchelon.py b/mobile_trunk_sim/echelon3/createEchelon.py index e061617..c174648 100644 --- a/mobile_trunk_sim/echelon3/createEchelon.py +++ b/mobile_trunk_sim/echelon3/createEchelon.py @@ -1,13 +1,9 @@ - -from email import contentmanager from .parameters import * from .classEchelon import * -if base_translation : - from .BaseController import * if not inverse : from .CableController import * - + from .ForceController import * def createEchelon(echelon,base,index,translation,rotation): @@ -20,6 +16,7 @@ def createEchelon(echelon,base,index,translation,rotation): 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) @@ -33,8 +30,8 @@ def createEchelon(echelon,base,index,translation,rotation): # adding Points and topology ######################################### - framesNode = echelon - + 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) @@ -52,8 +49,8 @@ def createEchelon(echelon,base,index,translation,rotation): ######################################### # 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=1e10 , angularStiffness=1e14) + + framesNode.addObject('RestShapeSpringsForceField', points=parameters.backboneEdges[0][0], external_points = index,external_rest_shape =base.getLinkPath() ,stiffness=1e10 , angularStiffness=1e14) ######################################### # Cables @@ -66,31 +63,31 @@ def createEchelon(echelon,base,index,translation,rotation): 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) - - ########################################## - # Visual model - ########################################## - visual = framesNode.addChild("VisualModel") - visual.addObject('MeshSTLLoader', name='loader', filename='meshes/trunk.stl',scale3d = [1000,1000,1000], rotation=[-90,-90,-90], - translation=[0, 0, -100]) - visual.addObject('MeshTopology', src='@loader') - visual.addObject('OglModel', name="renderer", src='@loader', color=[0.2,0.2,0.2,1.0]) - visual.addObject('RigidMapping', input=frames.getLinkPath(),index=index) - - ########################################## - # Colision model - ########################################## - - collison_model = framesNode.addChild("CollisionModel") - echelon_collision = collison_model.addChild("EchelonCollision") - echelon_collision.addObject('MeshSTLLoader', name='loader', filename='meshes/trunk.stl', scale3d=[1000, 1000, 1000]) - echelon_collision.addObject('MeshTopology', src='@loader') - #echelon_collision.addObject('MechanicalObject') - #echelon_collision.addObject('TriangleCollisionModel', group=0) - #echelon_collision.addObject('LineCollisionModel',group=0) - #echelon_collision.addObject('PointCollisionModel', group=0) - return parameters \ No newline at end of file + # ########################################## + # # Visual model + # ########################################## + # visual = framesNode.addChild("VisualModel") + # visual.addObject('MeshSTLLoader', name='loader', filename='meshes/trunk.stl',scale3d = [1000,1000,1000], rotation=[-90,-90,-90], + # translation=[0, 0, -100]) + # visual.addObject('MeshTopology', src='@loader') + # visual.addObject('OglModel', name="renderer", src='@loader', color=[0.2,0.2,0.2,1.0]) + # visual.addObject('RigidMapping', input=frames.getLinkPath(),index=index) + + # ########################################## + # # Colision model + # ########################################## + + # collison_model = framesNode.addChild("CollisionModel") + # echelon_collision = collison_model.addChild("EchelonCollision") + # echelon_collision.addObject('MeshSTLLoader', name='loader', filename='meshes/trunk.stl', scale3d=[1000, 1000, 1000]) + # echelon_collision.addObject('MeshTopology', src='@loader') + # #echelon_collision.addObject('MechanicalObject') + # #echelon_collision.addObject('TriangleCollisionModel', group=0) + # #echelon_collision.addObject('LineCollisionModel',group=0) + # #echelon_collision.addObject('PointCollisionModel', group=0) + return parameters,cables \ No newline at end of file diff --git a/mobile_trunk_sim/echelon3/parameters.py b/mobile_trunk_sim/echelon3/parameters.py index c553db2..73d84ac 100644 --- a/mobile_trunk_sim/echelon3/parameters.py +++ b/mobile_trunk_sim/echelon3/parameters.py @@ -12,7 +12,7 @@ inverse = False # inverse model base_translation = True rate_limiter = True # limit the movement of the target -typeControl = 'force' +typeControl = 'displacement' ros = False diff --git a/mobile_trunk_sim/summit_xl.py b/mobile_trunk_sim/summit_xl.py index 12cb45c..7aa0793 100644 --- a/mobile_trunk_sim/summit_xl.py +++ b/mobile_trunk_sim/summit_xl.py @@ -25,10 +25,11 @@ def Chassis(): sensorPositions = [[0., 0.28*1000 ,0.], # 2d lazer [0,0.275*1000,-0.22*1000], # gps - [0., 0.26*1000, 0.19*1000] #trunk ] - sensorName=["lazer", "gps", "trunk"] + sensorName=["lazer", "gps"] + + trunkPosition = [0., 0.26*1000, 0.32*1000,-0.5, -0.5, -0.5 , 0.5 ] ######################################### #Add chassis mechanical Object @@ -51,7 +52,7 @@ def Chassis(): #capteur sensor = self.addChild("FixedSensor") - sensor.addObject('MechanicalObject', name="angles", template="Vec1d", position=[0,0,0, 0]) + sensor.addObject('MechanicalObject', name="angles", template="Vec1d", position=[0,0,0]) sensor.addObject('UniformMass', name="vertexMass", vertexMass=[totalMass, volume, inertiaMatrix[:]]) ######################################### @@ -62,7 +63,7 @@ def Chassis(): sensor.addObject('ArticulatedHierarchyContainer') - for i in range(3): + 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") @@ -92,11 +93,12 @@ def Chassis(): output=wheels.position.getLinkPath()) ######################################### - #AAdd sensors + # add sensors ######################################### + sensors = self.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]], + 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[:]]) @@ -150,6 +152,7 @@ def Chassis(): ######################################### # collision models ######################################### + collison_model = wheels.addChild("CollisionModel") for i in range(4): wheel_collision = collison_model.addChild("WheelCollision{0}".format(i)) @@ -161,8 +164,19 @@ def Chassis(): wheel_collision.addObject('PointCollisionModel', group=0) wheel_collision.addObject('RigidMapping', input=self.Wheels.position.getLinkPath(), index=i+1) + ######################################### + # add Trunk + ######################################### + + trunk = self.addChild("Trunk") + trunk.addObject("MechanicalObject", name = "position", template="Rigid3d", + position=trunkPosition, + showObject=True,showObjectScale = 30) + trunk.addObject('RigidRigidMapping',name='mapping', input=self.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], @@ -211,12 +225,12 @@ def createScene(rootNode): scene = Scene(rootNode) scene.addMainHeader() - scene.VisualStyle.displayFlags = 'showCollisionModels showForceFields' + scene.VisualStyle.displayFlags = 'hideBehaviorModels showForceFields showCollisionModels showInteractionForceFields' scene.addObject('DefaultVisualManagerLoop') scene.dt = 0.001 scene.gravity = [0., -9810., 0.] - scene.Modelling.addObject('EulerImplicitSolver') + scene.Modelling.addObject('EulerImplicitSolver',rayleighStiffness=0.01, rayleighMass=0, vdamping=0.1) solver = scene.Modelling.addObject('SparseLDLSolver',name = 'SparseLDLSolver',template="CompressedRowSparseMatrixMat3x3d") scene.Modelling.addObject('GenericConstraintCorrection' , solverName='SparseLDLSolver') @@ -246,7 +260,11 @@ def createScene(rootNode): ######################################## arm = rootNode.Modelling.SummitXL.Chassis.addChild('Arm') - connection = rootNode.Modelling.SummitXL.Chassis.Sensors.position - createEchelon(arm,connection,3,[0., 0.26*1000, 0.19*1000],[-90,-90,0]) + connection = rootNode.Modelling.SummitXL.Chassis.Trunk.position + parameters, cables = createEchelon(arm,connection,0,[0., 0.26*1000, 0.32*1000],[-90,-90,0]) + 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