Skip to content

Commit

Permalink
Merge pull request #8 from AlessandriniAntoine/coupling_trunk_and_sum…
Browse files Browse the repository at this point in the history 
…mit_xl

Coupling trunk and summit xl
  • Loading branch information
@samFabrice
samFabrice authored Jul 19, 2022
2 parents 4e2c542 + 7465831 commit 6dfce86
Show file tree
Hide file tree
Showing 8 changed files with 132 additions and 328 deletions.
38 changes: 0 additions & 38 deletions mobile_trunk_sim/echelon3/BaseController.py
View file

This file was deleted.

172 changes: 37 additions & 135 deletions mobile_trunk_sim/echelon3/CableController.py
View file
Original file line number Diff line number Diff line change
@@ -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
##### 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]}')
76 changes: 35 additions & 41 deletions mobile_trunk_sim/echelon3/ForceController.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -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}')

68 changes: 0 additions & 68 deletions mobile_trunk_sim/echelon3/SpringConnection.py
View file

This file was deleted.

Loading

0 comments on commit 6dfce86

Please sign in to comment.