def onBeginAnimationStep(self, deltaTime):
## Please feel free to add an example for a simple usage in /home/trs/sofa/build/unstable//home/trs/sofa/src/sofa/applications/plugins/SofaPython/scn2python.py
# self.joints = self.robot_pos[self.robot_step,1:8]
self.Instrument.getObject('mecha').position = geo.arrToStr(self.robot_pos[self.robot_step,1:8]*[100,100,100,1,1,1,1])
self.robot_step += 1
# self.updateRobot()
return 0
def onEndAnimationStep(self, deltaTime):
## Please feel free to add an example for a simple usage in /home/trs/sofa/build/unstable//home/trs/sofa/src/sofa/applications/plugins/SofaPython/scn2python.py
pos = np.array(self.Tabletop.getObject('mecha').position)
if use_network:
robot_pos = self.robot_pos[self.robot_step, 1:8]
updated_pos = self.net(
torch.tensor(np.append(robot_pos,
pos)).float()).detach().numpy()
self.Tabletop.getObject('mecha').position = geo.arrToStr(
updated_pos)
self.f.write(
str(self.time) + ',' + geo.arrToStr(pos, delimiter=',') + '\n')
# self.robot_step += 1
self.time += self.rootNode.findData('dt').value
return 0
def onBeginAnimationStep(self, deltaTime):
## Please feel free to add an example for a simple usage in /home/trs/sofa/build/unstable//home/trs/sofa/src/sofa/applications/plugins/SofaPython/scn2python.py
if (self.robot_step < self.robot_pos.shape[0]-1):
if (self.partial_step < time_scale):
difference = self.robot_pos[self.robot_step+1,1:8] - self.robot_pos[self.robot_step,1:8]
update = float(self.partial_step/time_scale) * difference + self.robot_pos[self.robot_step,1:8]
self.Instrument.getObject('mecha').position = geo.arrToStr(update)
self.partial_step += 1
else:
self.robot_step += 1
self.Instrument.getObject('mecha').position = geo.arrToStr(self.robot_pos[self.robot_step,1:8])
self.partial_step = 1
else:
self.rootNode.getRootContext().animate = False
return 0
def __init__(self, node, commandLineArguments):
print("Command line arguments for python : " +
str(commandLineArguments))
data_file = int(commandLineArguments[1])
self.time_scale = int(all_time_steps[data_file] * 10000)
print(self.time_scale)
self.partial_step = self.time_scale
self.folder_name = 'data' + str(data_file)
try:
Path(self.folder_name).mkdir(mode=0o777, parents=False)
except OSError:
print("Model path exists")
#folder_name = 'calibration'
# self.robot_pos = np.genfromtxt('../dataset/test/' + 'data_cartesian_processed.csv', delimiter=',')
self.robot_pos = np.genfromtxt(
'../dataset/2019-10-09-GelPhantom1/dvrk/' + self.folder_name +
'_robot_cartesian_processed_interpolated.csv',
delimiter=',')
self.createGraph(node)
self.Instrument.getObject('mecha').position = geo.arrToStr(
self.robot_pos[self.robot_step, 1:8])
self.grid_order = np.loadtxt('fine_grid_order.txt').astype(int)
self.grid_top = self.grid_order.reshape(25, 9, 9)[:, -1, :]
self.grid_top = self.grid_top.reshape(-1)
def __init__(self, node, commandLineArguments) :
self.count = 0
self.commandLineArguments = commandLineArguments
print("Command line arguments for python : "+str(commandLineArguments))
# self.robot_pos = np.genfromtxt('../dataset/test/' + 'data_cartesian_processed.csv', delimiter=',')
self.robot_pos = np.genfromtxt('../dataset/2019-10-09-GelPhantom1/dvrk/' + folder_name + '_robot_cartesian_processed_interpolated.csv', delimiter=',')
self.createGraph(node)
self.Instrument.getObject('mecha').position = geo.arrToStr(self.robot_pos[self.robot_step,1:8])
def __init__(self, node, commandLineArguments) :
self.count = 0
self.commandLineArguments = commandLineArguments
print("Command line arguments for python : "+str(commandLineArguments))
# self.robot.LoadRobot('/home/jieying/catkin_ws/src/cisst-saw/sawIntuitiveResearchKit/share/deprecated/dvpsm.rob')
self.robot_pos = np.genfromtxt('../dataset/2019-07-30/' + 'position_cartesian_processed.csv', delimiter=',')
# self.joints = self.robot_pos[self.robot_step,1:8]
# self.robot_step += 1
self.createGraph(node)
self.Instrument.getObject('mecha').position = geo.arrToStr(self.robot_pos[self.robot_step,1:8]*[1000,1000,1000,1,1,1,1])
def __init__(self, node, commandLineArguments):
self.count = 0
self.commandLineArguments = commandLineArguments
print("Command line arguments for python : " +
str(commandLineArguments))
self.robot_pos = np.genfromtxt('../dataset/2019-08-08-Lego/' +
dataset +
'_robot_cartesian_processed.csv',
delimiter=',')
self.createGraph(node)
self.Instrument.getObject('mecha').position = geo.arrToStr(
self.robot_pos[self.robot_step, 1:8])
def onKeyPressed(self, c):
## usage e.g.
print(c, 'has been pressed')
pos = np.array(self.Cylinder.getObject('Cyl').position)
print('pos is', pos)
if c == "Q":
newPos = pos + np.array([1, 0, 0, 0, 0, 0, 0])
newPos = geo.arrToStr(newPos)
print(newPos)
self.Cylinder.getObject('Cyl').position = newPos
if c == "W":
newPos = pos + np.array([-1, 0, 0, 0, 0, 0, 0])
newPos = geo.arrToStr(newPos)
self.Cylinder.getObject('Cyl').position = newPos
if c == "A":
newPos = pos + np.array([0, 1, 0, 0, 0, 0, 0])
newPos = geo.arrToStr(newPos)
self.Cylinder.getObject('Cyl').position = newPos
if c == "D":
newPos = pos + np.array([0, -1, 0, 0, 0, 0, 0])
newPos = geo.arrToStr(newPos)
self.Cylinder.getObject('Cyl').position = newPos
if c == "Z":
newPos = pos + np.array([0, 0, 1, 0, 0, 0, 0])
newPos = geo.arrToStr(newPos)
self.Cylinder.getObject('Cyl').position = newPos
if c == "X":
newPos = pos + np.array([0, 0, -1, 0, 0, 0, 0])
newPos = geo.arrToStr(newPos)
self.Cylinder.getObject('Cyl').position = newPos
if c == "T":
delta_q = geo.eulerToQuaternion([0, 0, 0, 0.1, 0, 0])
print(delta_q)
newPos = geo.q_mult(pos[0], delta_q)
print(newPos)
self.Cylinder.getObject('Cyl').position = geo.arrToStr(newPos)
if c == "Y":
delta_q = geo.eulerToQuaternion([0, 0, 0, -0.1, 0, 0])
newPos = geo.q_mult(pos[0], delta_q)
self.Cylinder.getObject('Cyl').position = geo.arrToStr(newPos)
if c == "G":
delta_q = geo.eulerToQuaternion([0, 0, 0, 0, 0.1, 0])
newPos = geo.q_mult(pos[0], delta_q)
self.Cylinder.getObject('Cyl').position = geo.arrToStr(newPos)
if c == "H":
delta_q = geo.eulerToQuaternion([0, 0, 0, 0, -0.1, 0])
newPos = geo.q_mult(pos[0], delta_q)
self.Cylinder.getObject('Cyl').position = geo.arrToStr(newPos)
if c == "V":
delta_q = geo.eulerToQuaternion([0, 0, 0, 0, 0, 0.1])
newPos = geo.q_mult(pos[0], delta_q)
self.Cylinder.getObject('Cyl').position = geo.arrToStr(newPos)
if c == "B":
delta_q = geo.eulerToQuaternion([0, 0, 0, 0, 0, -0.1])
newPos = geo.q_mult(pos[0], delta_q)
self.Cylinder.getObject('Cyl').position = geo.arrToStr(newPos)
if c == "E":
self.Cylinder.getObject('Cyl').velocity = '0 0 0 0 0 0'
return 0
def reset(self):
## Please feel free to add an example for a simple usage in /home/trs/sofa/build/unstable//home/trs/sofa/src/sofa/applications/plugins/SofaPython/scn2python.py
self.robot_step = 0
self.Instrument.getObject('mecha').position = geo.arrToStr(self.robot_pos[self.robot_step,1:8])
return 0
def onKeyPressed(self, c):
## usage e.g.
vel = np.array(self.Link2L.getObject('Link2L').velocity)
print(vel.shape)
if c == "Q":
newVel = vel + np.array([self.scale, 0, 0, 0, 0, 0])
newVel = geo.arrToStr(newVel)
self.Link2L.getObject('Link2L').velocity = newVel
if c == "W":
newVel = vel + np.array([-self.scale, 0, 0, 0, 0, 0])
newVel = geo.arrToStr(newVel)
self.Link2L.getObject('Link2L').velocity = newVel
if c == "A":
newVel = vel + np.array([0, self.scale, 0, 0, 0, 0])
newVel = geo.arrToStr(newVel)
self.Link2L.getObject('Link2L').velocity = newVel
if c == "D":
newVel = vel + np.array([0, -self.scale, 0, 0, 0, 0])
newVel = geo.arrToStr(newVel)
self.Link2L.getObject('Link2L').velocity = newVel
if c == "Z":
newVel = vel + np.array([0, 0, self.scale, 0, 0, 0])
newVel = geo.arrToStr(newVel)
self.Link2L.getObject('Link2L').velocity = newVel
if c == "X":
newVel = vel + np.array([0, 0, -self.scale, 0, 0, 0])
newVel = geo.arrToStr(newVel)
self.Link2L.getObject('Link2L').velocity = newVel
if c == "T":
newVel = vel + np.array([0, 0, 0, self.scale, 0, 0])
newVel = geo.arrToStr(newVel)
self.Link2L.getObject('Link2L').velocity = geo.arrToStr(newVel)
if c == "Y":
newVel = vel + np.array([0, 0, 0, -self.scale, 0, 0])
newVel = geo.arrToStr(newVel)
self.Link2L.getObject('Link2L').velocity = geo.arrToStr(newVel)
if c == "G":
newVel = vel + np.array([0, 0, 0, 0, self.scale, 0])
newVel = geo.arrToStr(newVel)
self.Link2L.getObject('Link2L').velocity = geo.arrToStr(newVel)
if c == "H":
newVel = vel + np.array([0, 0, 0, 0, -self.scale, 0])
print(newVel.shape)
newVel = geo.arrToStr(newVel)
self.Link2L.getObject('Link2L').velocity = geo.arrToStr(newVel)
if c == "V":
newVel = vel + np.array([0, 0, 0, 0, 0, self.scale])
newVel = geo.arrToStr(newVel)
self.Link2L.getObject('Link2L').velocity = geo.arrToStr(newVel)
if c == "B":
newVel = vel + np.array([0, 0, 0, 0, 0, -self.scale])
newVel = geo.arrToStr(newVel)
self.Link2L.getObject('Link2L').velocity = geo.arrToStr(newVel)
if c == "E":
self.Link2L.getObject('Link2L').velocity = '0 0 0 0 0 0'
return 0
