def __init__(self, subj_data):
self.p2 = dvrk.psm('PSM2')
self.m2 = dvrk.mtm('MTMR')
self.m2.set_wrench_body_orientation_absolute(True)
self.c = dvrk.console()
self.robot_state = False # initialize the flag that helps with switch the robot state
filename = 'Subj' + str(subj_data[0])
if subj_data[1] == 0:
filename = filename + '_nohaptics'
elif subj_data[1] == 1:
filename = filename + '_haptics'
else:
filename = filename + '_manual'
if subj_data[2] == 0:
filename = filename + '_train'
else:
filename = filename + '_test'
if subj_data[3] == 0:
filename = filename + '_ef50'
elif subj_data[3] == 1:
filename = filename + '_ds10'
elif subj_data[3] == 2:
filename = filename + '_ef30'
else:
filename = filename + '_ds30'
self.name = filename
def init_dvrk_home():
# init dvrk objject
p = dvrk.psm('PSM1')
print "Initialisation complete, will home the PSM"
p.home()
time.sleep(0.25)
return p
def __init__(self, robotName):
self._robot_name = robotName
self._robot = psm(self._robot_name)
self._last_axes = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self._last_buttons = [0, 0]
self.previous_mouse_buttons = [0, 0]
rospy.Subscriber('/spacenav/joy', Joy, self.joy_callback)
def main():
rospy.init_node('controller', anonymous=True)
robot = psm('PSM1')
rate = rospy.Rate(100) # 10hz
robot.home()
position_start = robot.get_current_position()
safe_pos = PyKDL.Frame( PyKDL.Rotation(PyKDL.Vector(0, 1, 0),
PyKDL.Vector(1, 0, 0),
PyKDL.Vector(0, 0,-1)),
PyKDL.Vector(-0.05,0,-0.10))
robot.move(safe_pos)
positions = [1,2,3,4]
# step_val_x = 0.003 #scan in increments of 3mm
# step_val_y = 0.006
# dir = 1
# for i in range(5):
# for j in range(10):
# robot.dmove(PyKDL.Vector(step_val_x * dir, 0, 0))
# robot.dmove(PyKDL.Vector(0, step_val_y, 0))
# dir *= -1
for pos in positions:
robot.move(safe_pos)
robot.dmove(PyKDL.Vector(0, 0.05, -0.05))
rate.sleep()
while not rospy.is_shutdown():
#print("Running")
rate.sleep()
def __init__(self,
world="/world",
listener=None,
traj_step_t=0.1,
max_acc=1,
max_vel=1,
max_goal_diff=0.02,
goal_rotation_weight=0.01,
max_q_diff=1e-6):
# TODO: correct these
base_link = 'PSM1_psm_base_link'
end_link = 'PSM1_tool_wrist_sca_ee_link_0'
planning_group = 'manipulator'
self.dvrk_arm = dvrk.psm('PSM1')
self.psm_initialized = False
rospy.Subscriber("/instructor_marker/feedback",
InteractiveMarkerFeedback, self.marker_cbback)
self.last_marker_frame = PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, 0),
PyKDL.Vector(0, 0, 0))
self.last_marker_trans = (0, 0, 0)
self.last_marker_rot = (0, 0, 0, 1)
super(CostarPSMDriver, self).__init__(base_link,
end_link,
planning_group,
dof=6)
def __init__(self, robotName):
self._robot_name = robotName
self._robot = psm(self._robot_name)
self._last_axes = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self._last_buttons = [0, 0]
self.previous_mouse_buttons = [0, 0]
rospy.Subscriber('/spacenav/joy', Joy, self.joy_callback)
def main():
rospy.init_node('controller', anonymous=True)
robot = psm('PSM1')
rate = rospy.Rate(100) # 10hz
robot.home()
position_start = robot.get_current_position()
safe_pos = PyKDL.Frame(
PyKDL.Rotation(PyKDL.Vector(0, 1, 0), PyKDL.Vector(1, 0, 0),
PyKDL.Vector(0, 0, -1)),
PyKDL.Vector(0.05, -0.04, -0.13))
robot.move(safe_pos)
step_val_x = 0.02
step_val_y = 0.005
step_val_z = 0.0001
dir = 1
z_dir = 1
for i in range(7):
for j in range(18):
if j > 45:
z_dir = -1
robot.dmove(PyKDL.Vector(0, step_val_y * dir, step_val_z * z_dir))
robot.dmove(PyKDL.Vector(-step_val_x, 0, 0))
dir *= -1
z_dir = 1
print("Scan complete")
def __init__(self, robot_name, config_file):
print("initializing recording for", robot_name)
print("have a flat surface below the robot")
self.data = []
self.tracker = False
self.info = {}
# Add checker for directory
strdate = time.strftime("%Y-%m-%d_%H-%M-%S")
self.folder = os.path.join("data", "{}_{}".format(robot_name, strdate))
os.mkdir(self.folder)
print("Created folder at {}".format(os.path.abspath(self.folder)))
self.arm = dvrk.psm(robot_name)
self.home()
tree = ET.parse(config_file)
root = tree.getroot()
xpath_search_results = root.findall("./Robot/Actuator[@ActuatorID='2']"
"/AnalygIn/VoltsToPosSI")
if len(xpath_search_results) == 1:
VoltsToPosSI = xpath_search_results[0]
else:
print("Error: There must be exactly Actuator 2")
sys.exit(1)
current_offset = float(VoltsToPosSI.get("Offset"))
self.info["Config File"] = config_file
self.info["Current Offset"] = current_offset
def __init__(self, cameraTransform, objPath, scale):
s = rospy.Service('probe2D', oct_15_demo.srv.Probe2D, self.probe2D)
# Set up subscribers
self.forceSub = rospy.Subscriber('/force_sensor_topic',
ForceSensorData, self.forceCb)
# self.forceSub = rospy.Subscriber('/atinetft/wrench',
# WrenchStamped, self.forceCb)
# self.forceSub = rospy.Subscriber('/dvrk/PSM2/wrench_body_current',
# WrenchStamped, self.forceCb)
self.organPoseSub = rospy.Subscriber('registration_pose', PoseStamped,
self.poseCb)
self.roiSub = rospy.Subscriber('stiffness_roi', RegionOfInterest,
self.roiCb)
self.cameraTransform = cameraTransform
self.organTransform = None
self.robot = psm('PSM2')
self.toolOffset = .02 # distance from pinching axle to center of orange nub
rate = 1000.0
# TODO make these values not hard coded
self.rate = rospy.Rate(rate) # 1000hz
self.resolvedRatesConfig = \
{ 'velMin': 2.0/1000,
'velMax': 30.0/1000,
'angVelMin': 1.0/180.0*3.14,
'angVelMax': 60.0/180.0*3.14,
'tolPos': 0.1/1000.0, # positional tolerance
'tolRot': 1.0/180.0*3.14, # rotational tolerance
'velRatio': 1, # the ratio of max velocity error radius to tolarance radius, this value >1
'rotRatio': 1,
'dt': 1.0/rate, # this is the time step of the system.
# if rate=1khz, then dt=1.0/1000. However,
# we don't know if the reality will be the same as desired rate
}
# TODO make these not hard-coded
self.maxDepth = 0.01 # Meters
self.maxForce = 800 # Not sure of this?
self.safeZ = .05 # Safe height above organ in meters
self.normalDistance = 0.005 # Meters
self.safeSpot = PyKDL.Frame()
self.safeSpot.p = PyKDL.Vector(0, 0.00, -0.05)
self.safeSpot.M = rotationFromVector(PyKDL.Vector(0, 0, -.1))
self.robot.move(self.safeSpot)
self.resetZRot()
self.rate.sleep()
self.force = None
self.baselineForce = None
# Get obj data
objData = makeTexturedObjData(objPath, scale)
self.uvToWorldConverter = UVToWorldConverter(objData)
self.roi = None
self.stiffness_img_size = 1000 # in pixels
def __init__(self, psmName, forceTopic, bufferSize=50):
rospy.init_node('continuous_palpation', anonymous=True)
self.f_buffer = deque([], bufferSize)
self.f_current = np.zeros((3))
self.trajectory = deque([])
# Set up subscibers
self.poseSub = rospy.Subscriber(name='set_continuous_palpation_goal',
data_class=PoseStamped,
callback=self.poseCB,
queue_size=1)
self.trajSub = rospy.Subscriber(
name='set_continuous_palpation_trajectory',
data_class=PoseArray,
callback=self.poseArrayCB,
queue_size=1)
self.forceSub = rospy.Subscriber(name=forceTopic,
data_class=WrenchStamped,
callback=self.forceCB,
queue_size=1)
# Set up publishers
self.trajStatusPub = rospy.Publisher(name='trajectory_length',
data_class=UInt32,
queue_size=1)
self.robot = psm(psmName)
# TODO make these values not hard coded
self.rate = rospy.Rate(1000) # 1000hz
self.forceProfile = \
{ 'period':0.5, # Seconds
'amplitude': 0.5, # Newtons
'fBiasMag': 0.7, # Newtons, biased force magnitude
'magnitudeMode': 'bias', # 'bias' or 'sine', or 'sine bias'
'controlDir':'default', # 'default' or 'surf normal'
'defaultDir':[0.0,0.0,1.0],
'admittanceGains':[ 100.0/1000,\
100.0/1000,\
100.0/1000] # force admittance gains, (meter/sec)/Newton
}
self.resolvedRatesConfig = \
{ 'velMin': 5.0/1000,
'velMax': 50.0/1000,
'angVelMin': 3.0/180.0*3.14,
'angVelMax': 60.0/180.0*3.14,
'tolPos': 0.5/1000, # positional tolerance
'tolRot': 1.0/180*3.14, # rotational tolerance
'velRatio': 10.0, # the ratio of max velocity error radius to tolarance radius, this value >1
'rotRatio': 5.0,
'dt': 1.0/1000, # this is the time step of the system.
# if rate=1khz, then dt=1.0/1000. However,
# we don't know if the reality will be the same as desired rate
}
self.run()
def configure(self, robot_name):
print rospy.get_caller_id(
), ' -> configuring dvrk_psm_test for ', robot_name
self.arm = dvrk.psm(robot_name)
self.markerArray = MarkerArray()
self.publisher = rospy.Publisher('/visualization_marker_array',
MarkerArray,
queue_size=100)
rospy.sleep(2)
def __init__(self,psm_name, totaltime_s,num_wayPoints,joints,joint_limits, Ts):
# create the psm
self.statSubscriber = rospy.Subscriber(psm_name+'/recState',String,self.__subCallback)
self.recState = ''
self.arm = dvrk.psm(psm_name)
self.time = totaltime_s
self.num_wayPoints = num_wayPoints
self.joints = joints
self.joint_limits = joint_limits
self.Ts = Ts
def init_dvrk_mc():
# init dvrk objject
p = dvrk.psm('PSM1')
# init MC objects
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = "psm_arm"
group = moveit_commander.MoveGroupCommander(group_name)
return p,group
def jigsaw_move_dvrk_arm(self):
# rospy.init_node('move_dvrk_arm')
p = dvrk.psm('PSM1')
p.home()
# Create a Python proxy for PSM1, name must match ros namespace
# p.move(0.001, 0)
tip_pos_x, tip_pos_y, tip_pos_z, frame_val = jigsaw_data_parser()
for i in range(0, 4):
p.move(
PyKDL.Vector(0.5 * tip_pos_x[i], 0.5 * tip_pos_y[i],
0.5 * tip_pos_z[i]))
print(tip_pos_x[i], tip_pos_y[i], tip_pos_z[i])
def main(psmName):
rospy.init_node('dvrk_registration', anonymous=True)
robot = psm(psmName)
frameRate = 15
slop = 1.0 / frameRate
cams = StereoCameras("left/image_rect",
"right/image_rect",
"left/camera_info",
"right/camera_info",
slop=slop)
tfSync = CameraSync(
'/stereo/left/camera_info',
topics=['/dvrk/' + psmName + '/position_cartesian_current'],
frames=[
psmName + '_psm_base_link', psmName + '_tool_wrist_link',
psmName + '_tool_wrist_caudier_link_shaft'
])
camModel = StereoCameraModel()
topicLeft = rospy.resolve_name("left/camera_info")
msgL = rospy.wait_for_message(topicLeft, CameraInfo, 10)
topicRight = rospy.resolve_name("right/camera_info")
msgR = rospy.wait_for_message(topicRight, CameraInfo, 10)
camModel.fromCameraInfo(msgL, msgR)
# Set up GUI
filePath = rospy.get_param('~registration_yaml')
print(filePath)
with open(filePath, 'r') as f:
data = yaml.load(f)
if any(k not in data
for k in ['H', 'minS', 'minV', 'maxV', 'transform', 'points']):
rospy.logfatal('dVRK Registration: ' + filePath +
' empty or malformed.')
quit()
cv2.namedWindow(_WINDOW_NAME)
transformOld = np.array(data['transform'])
toolOffset = data[
'toolOffset'] # distance from pinching axle to center of orange nub
points = np.array(
data['points']) # Set of points in robots frame to register against
# Wait for registration to start
displayRegistration(cams, camModel, toolOffset, transformOld, tfSync)
print('Done')
cv2.destroyAllWindows()
def main():
rospy.init_node('controller', anonymous=True)
robot = psm('PSM1')
rate = rospy.Rate(100) # 10hz
robot.home()
position_start = robot.get_current_position()
safe_pos = PyKDL.Frame(
PyKDL.Rotation(PyKDL.Vector(0, 1, 0), PyKDL.Vector(1, 0, 0),
PyKDL.Vector(0, 0, -1)), PyKDL.Vector(0, 0, -0.1135))
robot.move(safe_pos)
positions = [1, 2, 3, 4]
for pos in positions:
robot.move(safe_pos)
robot.dmove(PyKDL.Vector(0, 0.05, -0.05))
rate.sleep()
def dvrk_data_write_to_file_single_arm(self):
rospy.init_node('read_write_data_dvrk')
rate = rospy.Rate(self.loop_rate)
# Create a Python proxy for PSM1, name must match ros namespace
p = dvrk.psm('PSM1')
# p.move_joint_one(0.05, 0)
# Location of file storage
# data_file_dir = "/home/aimlabx/PycharmProjects/dvrk_automated_suturing/data/dvrk_joint_data.csv"
csv = open(self.file_dir, "a")
# Initialization of rpy and pos
current_rpy = np.zeros(3)
current_pos = np.zeros(3)
# For writing the heading to the csv file
if self.file_heading_exits == 0:
p.home()
column_title = "Rot R, Rot P, Rot Y, Pos X, Pos Y, Pos Z \n"
csv.write(column_title)
self.file_heading_exits = 1
# While loop to read and write data continuously
while not rospy.is_shutdown():
# Read joint positions and angles of dvrk arm
current_pose = p.get_current_position()
current_rpy = current_pose.M.GetRPY()
current_pos = current_pose.p
# print("\nThe current joint position is ", current_pos, "\n")
# print("\nThe current joint position is ", current_vel, "\n")
# Initialize the row data with joint 0 position
row_data = str(current_rpy[0])
# Write the data to file
# For writing each end effector angle value
for i in range(1, 3):
row_data += "," + str(current_rpy[i])
# print("\n I value is ", i, "\n")
if i == 2:
# For writing each end effector position value
row_data += "," + str(current_pos[0])
print "Row data is ", row_data
for j in range(1, 3):
row_data += "," + str(current_pos[j])
# New set of data starts in a new line
if j == 2:
row_data = row_data + "\n"
# Finally write the data in row_data to the csv file
csv.write(row_data)
# Decides at what rate the data is written to the csv file
rate.sleep()
def __init__(self,
cam,
camTransform,
dvrkName,
forceTopic,
draw="bar",
masterWidget=None,
parent=None):
super(ForceOverlayWidget, self).__init__(cam, parent=parent)
self.masterWidget = masterWidget
if self.masterWidget == None:
self.robot = psm(dvrkName)
else:
self.robot = self.masterWidget.robot
self.cameraTransform = camTransform
self.drawType = draw
rospy.Subscriber(forceTopic, WrenchStamped, self.forceCB)
def main(screen):
screen.nodelay(1)
# this sets the increment for our displacement
delta = 0.001
total_translation = 0.0
translation = PyKDL.Vector(0.0,0.0,delta)
'''define the waypoint positions for the PSMs for this load test'''
cart1 = PyKDL.Vector(0.116,-0.090,-0.083)
rot1 = PyKDL.Rotation()
rot1 = rot1.Quaternion(0.694,0.0113,0.719,-0.005)
pos1 = PyKDL.Frame(rot1,cart1)
p2 = dvrk.psm('PSM2')
c = dvrk.console()
# set our rate to 30hz
rate = rospy.Rate(30)
print 'homing to position...'
c.teleop_stop()
#p2.open_jaw()
#p2.move(pos1)
p2.close_jaw()
while True:
if screen.getch() == ord('w'):
p2.dmove(translation)
total_translation = total_translation + delta
if screen.getch() == ord('d'):
print 'displacement is: '
print total_translation
if screen.getch() == ord('q'):
print 'quitting'
break
c.teleop_start()
def __init__(self, startp, endp):
self.organPoseSub = rospy.Subscriber('registration_pose', PoseStamped,
self.poseCb)
self.organTransform = None
self.robot = psm('PSM2')
self.toolOffset = .02 # distance from pinching axle to center of orange nub
rate = 1000.0
# TODO make these values not hard coded
self.rate = rospy.Rate(rate) # 1000hz
self.resolvedRatesConfig = \
{ 'velMin': 2.0/1000,
'velMax': 30.0/1000,
'angVelMin': 1.0/180.0*3.14,
'angVelMax': 60.0/180.0*3.14,
'tolPos': 0.1/1000.0, # positional tolerance
'tolRot': 1.0/180.0*3.14, # rotational tolerance
'velRatio': 1, # the ratio of max velocity error radius to tolarance radius, this value >1
'rotRatio': 1,
'dt': 1.0/rate, # this is the time step of the system.
# if rate=1khz, then dt=1.0/1000. However,
# we don't know if the reality will be the same as desired rate
}
# TODO make these not hard-coded
self.maxDepth = 0.01 # Meters
self.maxForce = 800 # Not sure of this?
self.safeZ = .05 # Safe height above organ in meters
self.normalDistance = 0.005 # Meters
self.safeSpot = PyKDL.Frame()
self.safeSpot.p = startp
self.safeSpot.M = fingertipConstraint(startp)
self.scanEndSpot = PyKDL.Frame()
self.scanEndSpot.p = endp
self.scanEndSpot.M = fingertipConstraint(endp)
self.robot.move(self.safeSpot)
self.resetZRot()
self.rate.sleep()
self.robot.move(self.scanEndSpot)
def dvrk_data_write_to_file_two_arms(self):
# rospy.init_node('read_write_data_dvrk')
rate = rospy.Rate(self.loop_rate)
# Create a Python proxy for PSM1, name must match ros namespace
p = dvrk.psm('PSM1')
# p.move_joint_one(0.05, 0)
# Location of file storage
# data_file_dir = "/home/aimlabx/PycharmProjects/dvrk_automated_suturing/data/dvrk_joint_data.csv"
csv = open(self.file_dir, "a")
# For writing the heading to the csv file
if self.file_heading_exits == 0:
p.home()
column_title = "Pos Joint 1, Pos Joint 2, Pos Joint 3, Pos Joint 4, Pos Joint 5, Pos Joint 6, " \
"Vel Joint 1, Vel Joint 2, Vel Joint 3, Vel Joint 4, Vel Joint 5, Vel Joint 6 \n"
csv.write(column_title)
self.file_heading_exits = 1
# While loop to read and write data continuously
while not rospy.is_shutdown():
# Read joint positions of dvrk arm
current_pos = p.get_current_joint_position()
# Read joint velocities of dvrk arm
current_vel = p.get_current_joint_velocity()
# print("\nThe current joint position is ", current_pos, "\n")
# print("\nThe current joint position is ", current_vel, "\n")
# Initialize the row data with joint 0 position
row_data = str(current_pos[0])
# Write the data to file
# For writing each joint position value
for i in range(1, 6):
row_data += "," + str(current_pos[i])
# print("\n I value is ", i, "\n")
if i == 5:
# For writing each joint velocity value
row_data += "," + str(current_vel[0])
for j in range(1, 6):
row_data += "," + str(current_vel[j])
# New set of data starts in a new line
if j == 5:
row_data = row_data + "\n"
# Finally write the data in row_data to the csv file
csv.write(row_data)
# Decides at what rate the data is written to the csv file
rate.sleep()
def main():
p=dvrk.psm('PSM1')
rate = rospy.Rate(250)
p.home()
#Home robot to zero
p.dmove_joint(np.array([0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0]))
#parameters of the identification task
rospack = rospkg.RosPack()
fileloc = rospack.get_path(dvrk_si) + '/test/6dof_31par_test1'
filename = fileloc +'Test.csv'
q= genfromtxt(filename, delimiter=',')
qt= q.transpose()
q_output=[None]*len(q[1][:])
def __init__(self, names):
self.name = names
self.pos = np.zeros(3)
self.rot = np.zeros(4)
self.arm_rot = np.zeros(4)
self.marker_data_pos = np.zeros((1, 3))
self.marker_data_rot = np.zeros((1, 4))
print(self.name)
if self.name[0:3] == 'ECM':
self.interface = dvrk.ecm(self.name[0])
else:
self.interface = dvrk.psm(self.name[0])
rospy.Subscriber(
'/vrpn_client_node/RigidBody' + str(self.name[1]) + '/pose',
gm.PoseStamped, self.handle_rotation)
rospy.Subscriber(
'/vrpn_client_node/RigidBody' + str(self.name[2]) + '/pose',
gm.PoseStamped, self.handle_rcm)
def __init__(self,
name="psm1",
world="/world",
listener=None,
traj_step_t=0.1,
max_acc=1,
max_vel=1,
max_goal_diff=0.02,
goal_rotation_weight=0.01,
max_q_diff=1e-6):
# TODO: correct these
base_link = 'PSM1_psm_base_link'
end_link = 'PSM1_tool_tip_link'
planning_group = 'manipulator'
self.dvrk_arm = dvrk.psm('PSM1')
super(CostarPSMDriver, self).__init__(base_link,
end_link,
planning_group,
dof=6)
def configure(self, robot_name, config_file, expected_interval):
self.expected_interval = expected_interval
self.config_file = config_file
# check that the config file is good
if not os.path.exists(self.config_file):
sys.exit("Config file \"{%s}\" not found".format(self.config_file))
# XML parsing, find current offset
self.tree = ET.parse(config_file)
root = self.tree.getroot()
# find Robot in config file and make sure name matches
xpath_search_results = root.findall("./Robot")
if len(xpath_search_results) == 1:
xmlRobot = xpath_search_results[0]
else:
sys.exit("Can't find \"Robot\" in configuration file {:s}".format(
self.config_file))
if xmlRobot.get("Name") == robot_name:
serial_number = xmlRobot.get("SN")
print(
"Successfully found robot \"{:s}\", serial number {:s} in XML file"
.format(robot_name, serial_number))
robotFound = True
else:
sys.exit(
"Found robot \"{:s}\" while looking for \"{:s}\", make sure you're using the correct configuration file!"
.format(xmlRobot.get("Name"), robot_name))
# now find the offset for joint 2, we assume there's only one result
xpath_search_results = root.findall(
"./Robot/Actuator[@ActuatorID='2']/AnalogIn/VoltsToPosSI")
self.xmlPot = xpath_search_results[0]
print("Potentiometer offset for joint 2 is currently: {:s}".format(
self.xmlPot.get("Offset")))
self.arm = dvrk.psm(arm_name=robot_name,
expected_interval=expected_interval)
def __init__(self,
world="/world",
listener=None,
traj_step_t=0.1,
max_acc=1,
max_vel=1,
max_goal_diff = 0.02,
goal_rotation_weight = 0.01,
max_q_diff = 1e-6):
# TODO: correct these
base_link = 'PSM1_psm_base_link'
end_link = 'PSM1_tool_wrist_sca_ee_link_0'
planning_group = 'manipulator'
self.dvrk_arm = dvrk.psm('PSM1')
self.psm_initialized = False
rospy.Subscriber("/instructor_marker/feedback", InteractiveMarkerFeedback, self.marker_cbback)
self.last_marker_frame = PyKDL.Frame(PyKDL.Rotation.RPY(0,0,0),PyKDL.Vector(0,0,0))
self.last_marker_trans = (0,0,0)
self.last_marker_rot = (0,0,0,1)
super(CostarPSMDriver, self).__init__(base_link,end_link,planning_group, dof=6)
def __init__(self, stlScale = 0.001, parent = None):
QtWidgets.QMainWindow.__init__(self, parent)
self.frame = QtWidgets.QFrame()
self.vl = QtWidgets.QVBoxLayout()
self.vtkWidget = QVTKRenderWindowInteractor(self.frame)
self.vl.addWidget(self.vtkWidget)
self.ren = vtk.vtkRenderer()
self.vtkWidget.GetRenderWindow().AddRenderer(self.ren)
self.iren = self.vtkWidget.GetRenderWindow().GetInteractor()
# Create sphere
source = vtk.vtkSphereSource()
source.SetCenter(0, 0, 0)
source.SetRadius(0.003)
sphereMapper = vtk.vtkPolyDataMapper()
sphereMapper.SetInputConnection(source.GetOutputPort())
self.sphereActor = vtk.vtkActor()
self.sphereActor.SetMapper(sphereMapper)
self.sphereActor.GetProperty().SetColor(1, 0, 0)
self.ren.AddActor(self.sphereActor)
# Read in STL
reader = vtk.vtkSTLReader()
reader.SetFileName('/home/biomed/october_15_ws/src/dvrk_vision/defaults/femur.stl')
scaler = vtk.vtkTransformFilter()
if vtk.VTK_MAJOR_VERSION <= 5:
scaler.SetInputConnection(reader.GetOutput())
else:
scaler.SetInputConnection(reader.GetOutputPort())
scaleTransform = vtk.vtkTransform()
scaleTransform.Identity()
scaleTransform.Scale(stlScale, stlScale, stlScale)
scaler.SetTransform(scaleTransform)
# Create a mapper
mapper = vtk.vtkPolyDataMapper()
if vtk.VTK_MAJOR_VERSION <= 5:
mapper.SetInput(scaler.GetOutput())
else:
mapper.SetInputConnection(scaler.GetOutputPort())
# Create an actor
self.actor = vtk.vtkActor()
self.actor.SetMapper(mapper)
# Read in camera registration
scriptDirectory = os.path.dirname(os.path.abspath(__file__))
filePath = os.path.join(scriptDirectory, '..', '..', 'defaults',
'registration_params.yaml')
with open(filePath, 'r') as f:
data = yaml.load(f)
self.camTransform = np.array(data['transform'])
# Add point cloud
self.pointCloud = VtkPointCloud()
tf = np.linalg.inv(self.camTransform)
transform = vtk.vtkTransform()
transform.SetMatrix(tf.ravel())
self.pointCloud.vtkActor.SetPosition(transform.GetPosition())
self.pointCloud.vtkActor.SetOrientation(transform.GetOrientation())
self.ren.AddActor(self.pointCloud.vtkActor)
self.ren.AddActor(self.actor)
poseSub = rospy.Subscriber("/stereo/registration_pose", PoseStamped, self.poseCallback)
self.ren.ResetCamera()
self.frame.setLayout(self.vl)
self.setCentralWidget(self.frame)
self.vtkThread = QThread()
self.actorMat = np.eye(4)
self.thread = QThread()
robot = psm('PSM2')
self.worker = Worker(robot)
self.worker.intReady.connect(self.onIntReady)
self.worker.moveToThread(self.thread)
self.worker.finished.connect(self.thread.quit)
self.thread.started.connect(self.worker.procCounter)
self.thread.start()
self.started = False
self.show()
self.iren.Initialize()
def pykdl_frame2str(frame):
(rx, ry, rz, rw) = frame.M.GetQuaternion()
message1 = "Position (x,y,z): {: 0.8f}, {: 0.8f}, {: 0.8f} Orientation(rx,ry,rz,rw): {: 0.8f}, {: 0.8f}, {: 0.8f}, {: 0.8f}"\
.format(frame.p.x(), frame.p.y(), frame.p.z(), rx,ry,rz,rw)
return message1
if __name__ == "__main__":
sleep_time = 1
psm3 = dvrk.psm('PSM3')
time.sleep(0.3)
psm3.home()
current_pose = psm3.get_current_position()
initial_orientation = current_pose.M
print("Psm3 current position")
print(pykdl_frame2str(current_pose))
answer = raw_input(
"position the psm3 in a safe place and write 'Y' to continue. ")
if answer != 'Y':
print("Exiting the program")
exit(0)
else:
print("Start moving arm 5 times")
def configure(self, robot_name):
# configuration
print(rospy.get_caller_id(), ' -> configuring dvrk_arm_test for ', robot_name)
self.arm = dvrk.psm(robot_name)
self.arm.close_jaw()
psm3.pos()
psm3.master()
#Getting the actual transformation matrix instead of the quaternion, or an error message
transform_obj.trasformation(nodo_obj.left_tf)
T_c_p0 = np.dot(
psm3.T_c_l,
np.dot(transform_obj.T_l_o,
np.dot(psm3.T_o_pb,
psm3.T_pb_p0))) #transformation from fulcrum to camera frame
T_p0_c = np.linalg.inv(T_c_p0) #transformation from camera frame to fulcrum
#T_o_p0 = np.dot(psm3.T_o_pb, psm3.T_pb_p0) # I do not need it at the moment, it is the transform from the fulcrum to the point o on the checkerboard
#T_c_p0= np.dot(self.T_c_l*self.T_o_pb) #I do not need it
psm3 = dvrk.psm(psm3.psmName) #this is a dvrk API class from psm.py
#psm1= dvrk.psm('PSM1') # this is the first arm
#psm2= dvrk.psm('PSM2') # this is the second arm
#############modifica per non rompere il robot un altra volta.
sleep(3) #to see if a can publish the base frame now
#psm3.set_joint_velocity_ratio(0.2)
#psm3.set_joint_acceleration_ratio(0.2)
#############
psm3.set_base_frame(
T_c_p0
) #this is the function I've Implemented to publish the base frame, but before you have to run the broadcaster in broadcaster_monel ( not sure, cannot verify it with the robot disconnected)
# plt.plot(q[:,0])
# plt.show()
print (q.shape)
a = q
print("data shape: {}".format(a.shape))
print(dof)
b = copy.deepcopy(a)
is_psm = robotname[0:3] == 'PSM'
if is_psm:
p = dvrk.psm(robotname)
for i in range(a.shape[0]):
b[i, 4] = 1.0186 * a[i, 4]
b[i, 5] = -0.8306 * a[i, 4] + 0.6089 * a[i, 5] + 0.6089 * a[i, 6]
b[i, 6] = -1.2177 * a[i, 5] +1.2177 * a[i, 6]
elif robotname[0:3] == 'MTM':
p = dvrk.mtm(robotname)
for i in range(a.shape[0]):
b[i, 2] = -a[i, 1] + a[i, 2]
b[i, 3] = 0.6697 * a[i, 1] - 0.6697 * a[i, 2] + a[i, 3]
a = b * scales
def configure(self, robot_name, expected_interval):
print_id('configuring dvrk_psm_test for %s' % robot_name)
self.expected_interval = expected_interval
self.arm = dvrk.psm(arm_name=robot_name,
expected_interval=expected_interval)
def configure(self, robot_name):
print(rospy.get_caller_id(), ' -> configuring dvrk_psm_test for ', robot_name)
self.arm = dvrk.psm(robot_name)
def run(self, calibrate, filename):
nb_joint_positions = 20 # number of positions between limits
nb_samples_per_position = 500 # number of values collected at each position
total_samples = nb_joint_positions * nb_samples_per_position
samples_so_far = 0
sleep_time_after_motion = 0.5 # time after motion from position to position to allow potentiometers to stabilize
sleep_time_between_samples = 0.01 # time between two samples read (potentiometers)
encoders = []
potentiometers = []
range_of_motion_joint = []
average_encoder = [] # all encoder values collected at a sample position used for averaging the values of that position
average_potentiometer = [] # all potentiometer values collected at a sample position used for averaging the values of that position
d2r = math.pi / 180.0 # degrees to radians
r2d = 180.0 / math.pi # radians to degrees
slopes = []
offsets = []
average_offsets = []
# Looking in XML assuming following tree structure
# config > Robot> Actuator > AnalogIn > VoltsToPosSI > Scale = ____ or Offset = ____
xmlVoltsToPosSI = {}
tree = ET.parse(filename)
root = tree.getroot()
robotFound = False
stuffInRoot = root.getchildren()
for index in range(len(stuffInRoot)):
if stuffInRoot[index].tag == "Robot":
currentRobot = stuffInRoot[index]
if currentRobot.attrib["Name"] == self._robot_name:
self._serial_number = currentRobot.attrib["SN"]
xmlRobot = currentRobot
print("Succesfully found robot \"", currentRobot.attrib["Name"], "\", Serial number: ", self._serial_number, " in XML file")
robotFound = True
else:
print("Found robot \"", currentRobot.attrib["Name"], "\", while looking for \"", self._robot_name, "\"")
if robotFound == False:
sys.exit("Robot tree could not be found in xml file")
# look for all VoltsToPosSI
stuffInRobot = xmlRobot.getchildren()
for index in range(len(stuffInRobot)):
child = stuffInRobot[index]
if child.tag == "Actuator":
actuatorId = int(child.attrib["ActuatorID"])
stuffInActuator = child.getchildren()
for subIndex in range(len(stuffInActuator)):
subChild = stuffInActuator[subIndex]
if subChild.tag == "AnalogIn":
stuffInAnalogIn = subChild.getchildren()
for subSubIndex in range(len(stuffInAnalogIn)):
subSubChild = stuffInAnalogIn[subSubIndex]
if subSubChild.tag == "VoltsToPosSI":
xmlVoltsToPosSI[actuatorId] = subSubChild
# set joint limits and number of axis based on arm type, using robot name
if ("").join(list(currentRobot.attrib["Name"])[:-1]) == "PSM": #checks to see if the robot being tested is a PSM
arm_type = "PSM"
lower_joint_limits = [-60.0 * d2r, -30.0 * d2r, 0.005, -170.0 * d2r, -170.0 * d2r, -170.0 * d2r, -170.0 * d2r]
upper_joint_limits = [ 60.0 * d2r, 30.0 * d2r, 0.235, 170.0 * d2r, 170.0 * d2r, 170.0 * d2r, 170.0 * d2r]
nb_axis = 7 #number of joints being tested
elif currentRobot.attrib["Name"] == "MTML":
arm_type = "MTM"
lower_joint_limits = [-15.0 * d2r, -10.0 * d2r, -10.0 * d2r, -180.0 * d2r, -80.0 * d2r, -40.0 * d2r, -100.0 * d2r]
upper_joint_limits = [ 35.0 * d2r, 20.0 * d2r, 10.0 * d2r, 80.0 * d2r, 160.0 * d2r, 40.0 * d2r, 100.0 * d2r]
nb_axis = 7
elif currentRobot.attrib["Name"] == "MTMR":
arm_type = "MTM"
lower_joint_limits = [-30.0 * d2r, -10.0 * d2r, -10.0 * d2r, -80.0 * d2r, -80.0 * d2r, -40.0 * d2r, -100.0 * d2r]
upper_joint_limits = [ 15.0 * d2r, 20.0 * d2r, 10.0 * d2r, 180.0 * d2r, 160.0 * d2r, 40.0 * d2r, 100.0 * d2r]
nb_axis = 7
elif currentRobot.attrib["Name"] == "ECM":
arm_type = "ECM"
lower_joint_limits = [-60.0 * d2r, -40.0 * d2r, 0.005, -80.0 * d2r]
upper_joint_limits = [ 60.0 * d2r, 40.0 * d2r, 0.230, 80.0 * d2r]
nb_axis = 4
if arm_type == "PSM":
this_arm = dvrk.psm(self._robot_name)
else:
this_arm = dvrk.arm(self._robot_name)
# resize all arrays
for axis in range(nb_axis):
encoders.append([])
offsets.append([])
potentiometers.append([])
average_encoder.append([])
average_offsets.append([])
average_potentiometer.append([])
range_of_motion_joint.append(math.fabs(upper_joint_limits[axis] - lower_joint_limits[axis]))
# Check that everything is working
time.sleep(2.0) # to make sure some data has arrived
if not self._data_received:
print("It seems the console for ", self._robot_name, " is not started or is not publishing the IO topics")
print("Make sure you use \"rosrun dvrk_robot dvrk_console_json\" with the -i option")
sys.exit("Start the dvrk_console_json with the proper options first")
print("The serial number found in the XML file is: ", self._serial_number)
print("Make sure the dvrk_console_json is using the same configuration file. Serial number can be found in GUI tab \"IO\".")
ok = input("Press `c` and [enter] to continue\n")
if ok != "c":
sys.exit("Quitting")
######## scale calibration
now = datetime.datetime.now()
now_string = now.strftime("%Y-%m-%d-%H:%M")
if calibrate == "scales":
print("Calibrating scales using encoders as reference")
# write all values to csv file
csv_file_name = 'pot_calib_scales_' + self._robot_name + '-' + self._serial_number + '-' + now_string + '.csv'
print("Values will be saved in: ", csv_file_name)
f = open(csv_file_name, 'wb')
writer = csv.writer(f)
header = []
for axis in range(nb_axis):
header.append("potentiometer" + str(axis))
for axis in range(nb_axis):
header.append("encoder" + str(axis))
writer.writerow(header)
# messages
input("To start with some initial values, you first need to \"home\" the robot. When homed, press [enter]\n")
if arm_type == "PSM":
input("Since you are calibrating a PSM, make sure there is no tool inserted. Please remove tool or calibration plate if any and press [enter]\n")
if arm_type == "ECM":
input("Since you are calibrating an ECM, remove the endoscope and press [enter]\n")
input("The robot will make LARGE MOVEMENTS, please hit [enter] to continue once it is safe to proceed\n")
for position in range(nb_joint_positions):
# create joint goal
joint_goal = []
for axis in range(nb_axis):
joint_goal.append(lower_joint_limits[axis] + position * (range_of_motion_joint[axis] / nb_joint_positions))
average_encoder[axis] = []
average_potentiometer[axis] = []
# move and sleep
if arm_type == "PSM":
this_arm.move_jaw(joint_goal[6], blocking = False)
this_arm.move_joint(numpy.array(joint_goal[0:6]))
else:
this_arm.move_joint(numpy.array(joint_goal))
time.sleep(sleep_time_after_motion)
# collect nb_samples_per_position at current position to compute average
for sample in range(nb_samples_per_position):
for axis in range(nb_axis):
average_potentiometer[axis].append(self._last_potentiometers[axis])
average_encoder[axis].append(self._last_joints[axis])
# log data
writer.writerow(self._last_potentiometers + self._last_joints)
time.sleep(sleep_time_between_samples)
samples_so_far = samples_so_far + 1
sys.stdout.write('\rProgress %02.1f%%' % (float(samples_so_far) / float(total_samples) * 100.0))
sys.stdout.flush()
# compute averages
for axis in range(nb_axis):
potentiometers[axis].append(math.fsum(average_potentiometer[axis]) / nb_samples_per_position)
encoders[axis].append(math.fsum(average_encoder[axis]) / nb_samples_per_position)
# at the end, return to home position
if arm_type == "PSM":
this_arm.move_jaw(0.0, blocking = False)
this_arm.move_joint(numpy.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]))
elif arm_type == "MTM":
this_arm.move_joint(numpy.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]))
elif arm_type == "ECM":
this_arm.move_joint(numpy.array([0.0, 0.0, 0.0, 0.0]))
# close file
f.close()
######## offset calibration
if calibrate == "offsets":
print("Calibrating offsets")
# write all values to csv file
csv_file_name = 'pot_calib_offsets_' + self._robot_name + '-' + self._serial_number + '-' + now_string + '.csv'
print("Values will be saved in: ", csv_file_name)
f = open(csv_file_name, 'wb')
writer = csv.writer(f)
header = []
for axis in range(nb_axis):
header.append("potentiometer" + str(axis))
writer.writerow(header)
# messages
print("Please home AND power off the robot first. Then hold/clamp your arm in zero position.")
if arm_type == "PSM":
print("For a PSM, you need to hold at least the last 4 joints in zero position. If you don't have a way to constrain the first 3 joints, you can still just calibrate the last 4. This program will ask you later if you want to save all PSM joint offsets");
input("Press [enter] to continue\n")
nb_samples = 10 * nb_samples_per_position
for sample in range(nb_samples):
for axis in range(nb_axis):
average_offsets[axis].append(self._last_potentiometers[axis] * r2d)
writer.writerow(self._last_potentiometers)
time.sleep(sleep_time_between_samples)
sys.stdout.write('\rProgress %02.1f%%' % (float(sample) / float(nb_samples) * 100.0))
sys.stdout.flush()
for axis in range(nb_axis):
offsets[axis] = (math.fsum(average_offsets[axis]) / (nb_samples) )
print("")
if calibrate == "scales":
print("index | old scale | new scale | correction")
for index in range(nb_axis):
# find existing values
oldScale = float(xmlVoltsToPosSI[index].attrib["Scale"])
# compute new values
correction = slope(encoders[index], potentiometers[index])
newScale = oldScale / correction
# display
print(" %d | % 04.6f | % 04.6f | % 04.6f" % (index, oldScale, newScale, correction))
# replace values
xmlVoltsToPosSI[index].attrib["Scale"] = str(newScale)
if calibrate == "offsets":
newOffsets = []
print("index | old offset | new offset | correction")
for index in range(nb_axis):
# find existing values
oldOffset = float(xmlVoltsToPosSI[index].attrib["Offset"])
# compute new values
newOffsets.append(oldOffset - offsets[index])
# display
print(" %d | % 04.6f | % 04.6f | % 04.6f " % (index, oldOffset, newOffsets[index], offsets[index]))
if arm_type == "PSM":
all = input("Do you want to save all joint offsets or just the last 4, press 'a' followed by [enter] to save all\n");
if all == "a":
print("This program will save ALL new PSM offsets")
for axis in range(nb_axis):
# replace values
xmlVoltsToPosSI[axis].attrib["Offset"] = str(newOffsets[axis])
else:
print("This program will only save the last 4 PSM offsets")
for axis in range(3, nb_axis):
# replace values
xmlVoltsToPosSI[axis].attrib["Offset"] = str(newOffsets[axis])
else:
for axis in range(nb_axis):
# replace values
xmlVoltsToPosSI[axis].attrib["Offset"] = str(newOffsets[axis])
save = input("To save this in new file press 'y' followed by [enter]\n")
if save == "y":
tree.write(filename + "-new")
print('Results saved in', filename + '-new. Restart your dVRK application with the new file!')
print('To copy the new file over the existing one: cp', filename + '-new', filename)
qt = q.transpose()
times = 1
speedscale=0.5
scale = 1
if times ==1:
a = q;
else:
a = np.zeros((len(q[:][1]),len(q[1][:])*times))
#Make twice the trajectory
for i in range(len(qt[1][:])):
a[i][:]=np.append(q[i][:],q[i][:])
p=dvrk.psm('PSM2')
r=rospy.Rate(200*speedscale)
p.home()
period_data = len(a[0][:])
data_cycle = len(a)
states = np.zeros((len(a)*len(a[1][:]),3*3))
j = 0
while j<data_cycle and not rospy.is_shutdown():
p.move_joint_some(np.array([0, 0, scale*a[j][0], 0, 0, 0]),np.array([0,1,2,3,4,5]))
#p.move_joint_some(np.array([scale*a[0][0], scale*a[1][0], scale*a[2][0]]),np.array([0,1,2]))
qt = q.transpose()
times = 1
speedscale=0.5
scale = 1
if times ==1:
a = q;
else:
a = np.zeros((len(q[:][1]),len(q[1][:])*times))
#Make twice the trajectory
for i in range(len(qt[1][:])):
a[i][:]=np.append(q[i][:],q[i][:])
p=dvrk.psm('PSM1')
r=rospy.Rate(200*speedscale)
p.home()
period_data = len(a)
data_cycle = len(a)
states = np.zeros((len(a),3*3))
j = 0
while j<1 and not rospy.is_shutdown():
p.move_joint_some(np.array([0, 0, scale*a[0], 0, 0, 0]),np.array([0,1,2,3,4,5]))
#p.move_joint_some(np.array([scale*a[0][0], scale*a[1][0], scale*a[2][0]]),np.array([0,1,2]))
