-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathwholeGripperController.py
109 lines (79 loc) · 3.58 KB
/
wholeGripperController.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import Sofa.Core
from Sofa.constants import *
import math
num = 2
# class WholeGripperController(Sofa.Core.Controller):
# def __init__(self, *a, **kw):
# Sofa.Core.Controller.__init__(self, *a, **kw)
# self.node = kw["node"]
# self.constraints = []
# self.dofs = []
# for i in range(1,num+1):
# self.dofs.append(self.node.getChild('finger' + str(i)).tetras)
# self.constraints.append(self.node.getChild('finger'+str(i)).cavity.SurfacePressureConstraint)
# self.centerPosY = 70
# self.centerPosZ = 0
# self.rotAngle = 0
# return
# def onKeypressedEvent(self,e):
# increment = 0.001 #0.01
# if e["key"] == Sofa.constants.Key.plus:
# for i in range(num):
# pressureValue = self.constraints[i].value.value[0] + increment
# if pressureValue > 1.5:
# pressureValue = 1.5
# self.constraints[i].value = [pressureValue]
# if e["key"] == Sofa.constants.Key.minus:
# for i in range(num):
# pressureValue = self.constraints[i].value.value[0] - increment
# if pressureValue < 0:
# pressureValue = 0
# self.constraints[i].value = [pressureValue]
class WholeGripperController(Sofa.Core.Controller):
def __init__(self, *a, **kw):
Sofa.Core.Controller.__init__(self, *a, **kw)
self.node = kw["node"]
self.constraints = []
self.dofs = []
for i in range(1,num+1):
#self.dofs.append(self.node.getChild('finger1').getChild('cavity' + str(i - 1)).cavity)
#self.dofs.append(self.node.getChild('cavity' + str(i)).tetras)
self.constraints.append(self.node.finger1.getChild('cavity' + str(i)).SurfacePressureConstraint)
self.centerPosY = 70
self.centerPosZ = 0
self.rotAngle = 0
return
def onKeypressedEvent(self,e):
increment = 0.001 #0.01
if e["key"] == Sofa.constants.Key.KP_4:
pressureValue = self.constraints[0].value.value[0] + increment
if pressureValue > 3.0:
pressureValue = 3.0
self.constraints[0].value = [pressureValue]
if e["key"] == Sofa.constants.Key.KP_5:
pressureValue = self.constraints[1].value.value[0] + increment
if pressureValue > 3.0:
pressureValue = 3.0
self.constraints[1].value = [pressureValue]
# if e["key"] == Sofa.constants.Key.KP_6:
# pressureValue = self.constraints[2].value.value[0] + increment
# if pressureValue > 3.0:
# pressureValue = 3.0
# self.constraints[2].value = [pressureValue]
if e["key"] == Sofa.constants.Key.KP_1:
pressureValue = self.constraints[0].value.value[0] - increment
if pressureValue < 0:
pressureValue = 0
self.constraints[0].value = [pressureValue]
if e["key"] == Sofa.constants.Key.KP_2:
pressureValue = self.constraints[1].value.value[0] - increment
if pressureValue < 0:
pressureValue = 0
self.constraints[1].value = [pressureValue]
# if e["key"] == Sofa.constants.Key.KP_3:
# pressureValue = self.constraints[2].value.value[0] - increment
# if pressureValue < 0:
# pressureValue = 0
# self.constraints[2].value = [pressureValue]