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