-
Notifications
You must be signed in to change notification settings - Fork 4
/
Copy pathexample_flat.py
159 lines (135 loc) · 3.86 KB
/
example_flat.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
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
import numpy as np
from splib3.numerics import Quat, Vec3
import math
from scipy.spatial.transform import Rotation as R
import os
import rospy
import rospkg
from mcr_sim import \
mcr_environment, mcr_instrument, mcr_emns, mcr_simulator, \
mcr_controller_sofa, mcr_magnet
# Calibration file for eMNS
cal_path = '../calib/Navion_2_Calibration_24-02-2020.yaml'
# Parameters instrument
young_modulus_body = 170e6 # (Pa)
young_modulus_tip = 21e6 # (Pa)
length_body = 0.5 # (m)
length_tip = 0.034 # (m)
outer_diam = 0.00133 # (m)
inner_diam = 0.0008 # (m)
length_init = .35
# Parameters environment
environment_stl = '../mesh/flat_models/flat_model_circles.stl'
# Parameter magnet
magnet_length = 4e-3 # (m)
magnet_id = 0.86e-3 # (m)
magnet_od = 1.33e-3 # (m)
magnet_remanence = 1.45 # (T)
# Parameter for beams
nume_nodes_viz = 600
num_elem_body = 30
num_elem_tip = 3
# Transforms
# Sofa sim frame in Navion
# Model in sofa sim frame
rot_env_sim = [0, 0, 0] # rpy angles
transl_env_sim = [0, 0, 0]
# transforms (translation , quat)
T_sim_mns = [
0., 0., 0.,
0., 0., 0., 1]
# Define entry pose in model
# starting pose in environment frame
T_start_env = [-0.04, 0.01, 0.002, 0., 0., 0., 1.]
X = Vec3(
T_start_env[0],
T_start_env[1],
T_start_env[2])
r = R.from_euler('xyz', rot_env_sim, degrees=True)
X = r.apply(X)
q = Quat.createFromEuler([
rot_env_sim[0]*math.pi/180,
rot_env_sim[1]*math.pi/180,
rot_env_sim[2]*math.pi/180])
qrot = Quat(
T_start_env[3],
T_start_env[4],
T_start_env[5],
T_start_env[6])
q.rotateFromQuat(qrot)
# starting pose sofa sim frame
T_start_sim = [
X[0]+transl_env_sim[0],
X[1]+transl_env_sim[1],
X[2]+transl_env_sim[2],
q[0], q[1], q[2], q[3]]
# transform environment
pos_env_sim = transl_env_sim
quat_env_sim = Quat.createFromEuler([
rot_env_sim[0]*np.pi/180,
rot_env_sim[1]*np.pi/180,
rot_env_sim[2]*np.pi/180])
T_env_sim = [
transl_env_sim[0],
transl_env_sim[1],
transl_env_sim[2],
quat_env_sim[0],
quat_env_sim[1],
quat_env_sim[2],
quat_env_sim[3]]
def createScene(root_node):
''' Build SOFA scene '''
# simulator
simulator = mcr_simulator.Simulator(
root_node=root_node)
# eMNS
navion = mcr_emns.EMNS(
name='Navion',
calibration_path=cal_path)
# environment
environment = mcr_environment.Environment(
root_node=root_node,
environment_stl=environment_stl,
T_env_sim=T_env_sim,
color=[1., 0., 0., 0.3])
# magnet
magnet = mcr_magnet.Magnet(
length=magnet_length,
outer_diam=magnet_od,
inner_diam=magnet_id,
remanence=magnet_remanence,
color=[.2, .2, .2, 1.])
# magnets on both ends of flexible segment
magnets = [0. for i in range(num_elem_tip)]
magnets[0] = magnet
magnets[1] = magnet
# instrument
instrument = mcr_instrument.Instrument(
name='mag_gw',
root_node=root_node,
length_body=length_body,
length_tip=length_tip,
outer_diam=outer_diam,
inner_diam=inner_diam,
young_modulus_body=young_modulus_body,
young_modulus_tip=young_modulus_tip,
magnets=magnets,
num_elem_body=num_elem_body,
num_elem_tip=num_elem_tip,
nume_nodes_viz=nume_nodes_viz,
T_start_sim=T_start_sim,
fixed_directions=[0, 0, 1, 0, 0, 0],
color=[0.2, .8, 1., 1.]
)
# ros-based controller
controller_sofa = mcr_controller_sofa.ControllerSofa(
name='ControllerSofa',
root_node=root_node,
e_mns=navion,
instrument=instrument,
environment=environment,
length_init=length_init,
T_sim_mns=T_sim_mns,
)
controller_sofa.print_insertion_length = False
root_node.addObject(controller_sofa)