def __init__(self, ):
self.arm_kinematic = self.ArmKinematic('PSM3')
#python api
self.api_psm3_arm = dvrk.arm('PSM3')
#tf
self.tf_world_psm3b = None
#subscribers
self.psm3_cartesian_subs = rospy.Subscriber(
"/dvrk/PSM3/position_cartesian_current", PoseStamped,
self.psm3_cartesian_callback)
self.psm1_suj_subs = rospy.Subscriber("/dvrk/PSM1/io/suj_clutch", Joy,
self.setup_button_psm1_callback)
#tf
self.tf_world_psm3b_subs = rospy.Subscriber(
"/pu_dvrk_tf/tf_world_psm3b", PoseStamped,
self.tf_world_psm3b_callback)
#psm offset
self.fix_orientation = PyKDL.Rotation.Quaternion(
0.20611444, -0.10502740, 0.60974223, 0.75809003)
#Upload homography between pixels and robot x,y
self.homography = None
with open("./vision_modules/misc/homography_data/homography.json",
"r") as f1:
self.homography = np.array(json.load(f1))
print("Retrieved homography")
print(self.homography)
def __init__(self, robotName):
self._robot_name = robotName
self._robot = arm(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 __init__(self, ):
#python api
self.api_psm3_arm = dvrk.arm('PSM3')
self.dvrk_controller = dvrk_tf_module(userId="test",
dst_path='./videos',
rig_name="pu_dvrk_cam")
#tf
self.tf_world_psm3b = PyKDL.Frame()
#subscribers
self.psm1_suj_subs = rospy.Subscriber("/dvrk/PSM1/io/suj_clutch", Joy,
self.setup_button_psm1_callback)
#tf
self.tf_world_psm3b_subs = rospy.Subscriber(
"/pu_dvrk_tf/tf_world_psm3b", PoseStamped,
self.tf_world_psm3b_callback)
self.ar_orientation = PyKDL.Rotation.Quaternion(
0.68175651, -0.27654879, 0.56096521, 0.37953506)
self.dvrk_controller.ar_orientation = self.ar_orientation
self.fix_orientation = PyKDL.Rotation.Quaternion(
0.20611444, -0.10502740, 0.60974223, 0.75809003)
self.base = Vector(*[0.23937060, -0.09208578, 0.05523316])
self.position1 = Vector(*[0.27518547, -0.18335637, 0.06959790])
self.position2 = Vector(*[0.23203641, -0.18565913, 0.06422155])
self.position3 = Vector(*[0.26621665, -0.11688039, 0.07422218])
self.position4 = Vector(*[0.21614252, -0.10163200, 0.07084345])
def __init__(self, ):
self.arm_kinematic = self.ArmKinematic('PSM3')
#python api
self.api_psm3_arm = dvrk.arm('PSM3')
#tf
self.tf_world_psm3b = None
#subscribers
self.psm3_cartesian_subs = rospy.Subscriber(
"/dvrk/PSM3/position_cartesian_current", PoseStamped,
self.psm3_cartesian_callback)
self.psm1_suj_subs = rospy.Subscriber("/dvrk/PSM1/io/suj_clutch", Joy,
self.setup_button_psm1_callback)
#tf
self.tf_world_psm3b_subs = rospy.Subscriber(
"/pu_dvrk_tf/tf_world_psm3b", PoseStamped,
self.tf_world_psm3b_callback)
#psm offset
self.offset = PyKDL.Vector(-0.009, -0.021, +0.069)
self.fix_orientation = PyKDL.Rotation.Quaternion(
0.20611444, -0.10502740, 0.60974223, 0.75809003)
def __init__(self, arm_name, camera_model, tf_listener):
rospy.loginfo('Configuring motion controller for ' + arm_name)
self.arm = dvrk.arm(arm_name)
self.arm_name = arm_name
self.arm_base = arm_name + "_base"
self.camera_model = camera_model
self.tf_listener = tf_listener
self.position_threshold = 1e-5
self.joint_threshold = 3 * math.pi / 180
def __init__(self, ):
self.api_arm = dvrk.arm('MTML')
#tf
self.tf_mtmlb_mtml = None
#subscribers
self.tf_subs = rospy.Subscriber(
"/dvrk/MTML/position_cartesian_current", PoseStamped,
self.tf_callback)
def main():
sleep_time = 1
psm3_arm = dvrk.arm('PSM3')
psm3_arm.home()
# #Set up the velocity and acceleration ratio of the psm3
# ratio = 0.01
# velocity_ratio_pub = rospy.Publisher("/dvrk/PSM3/set_joint_velocity_ratio",Float64,latch=True, queue_size=1)
# acceleration_ratio_pub = rospy.Publisher("/dvrk/PSM3/set_joint_acceleration_ratio",Float64,latch=True, queue_size=1)
# velocity_ratio_pub.publish(Float64(ratio))
# acceleration_ratio_pub.publish(Float64(ratio))
answer = raw_input(
"Make sure the PSM3 will not crash with the programmed movements and write 'Y' to continue. "
)
if answer != 'Y':
print("Exiting the program")
exit(0)
for i in range(2):
square_trajectory = [[[0.21134809, -0.12155905, 0.10276267],
[0.26790356, -0.12201603, 0.10604522]],
[[0.26790356, -0.12201603, 0.10604522],
[0.21134809, -0.12155905, 0.10276267]],
[[0.21134809, -0.12155905, 0.10276267],
[0.27185202, -0.14776761, 0.09245916]],
[[0.27185202, -0.14776761, 0.09245916],
[0.21329086, -0.14354401, 0.08516887]],
[[0.21329086, -0.14354401, 0.08516887],
[0.21134809, -0.12155905, 0.10276267]]]
for init_pt, end_pt in square_trajectory:
init_pt = np.array(init_pt)
end_pt = np.array(end_pt)
trajectory = generate_trajectory(init_pt, end_pt, n=2)
for idx, c in enumerate(range(trajectory.shape[1])):
next_position = PyKDL.Vector(
trajectory[0, c],
trajectory[1, c],
trajectory[2, c],
)
print("position {:}".format(idx), next_position)
psm3_arm.move(PyKDL.Vector(next_position))
def __init__(self, ):
self.arm_kinematic = self.ArmKinematic('PSM3')
#python api
self.api_psm3_arm = dvrk.arm('PSM3')
#tf
self.tf_world_psm3b = PyKDL.Frame()
#subscribers
self.psm3_cartesian_subs = rospy.Subscriber(
"/dvrk/PSM3/position_cartesian_current", PoseStamped,
self.psm3_cartesian_callback)
self.psm1_suj_subs = rospy.Subscriber("/dvrk/PSM1/io/suj_clutch", Joy,
self.setup_button_psm1_callback)
#tf
self.tf_world_psm3b_subs = rospy.Subscriber(
"/pu_dvrk_tf/tf_world_psm3b", PoseStamped,
self.tf_world_psm3b_callback)
def __init__(self, ):
self.arm_kinematic = self.ArmKinematic('PSM3')
#python api
self.api_psm3_arm = dvrk.arm('PSM3')
#subscribers
self.psm3_cartesian_subs = rospy.Subscriber(
"/dvrk/PSM3/position_cartesian_current", PoseStamped,
self.psm3_cartesian_callback)
self.psm1_suj_subs = rospy.Subscriber("/dvrk/PSM1/io/suj_clutch", Joy,
self.setup_button_psm1_callback)
self.tf_world_psm3b_pub = rospy.Publisher("/pu_dvrk_tf/tf_world_psm3b",
PoseStamped,
queue_size=5)
#publisher
self.multi_arr_pub = rospy.Publisher(
"/pu_dvrk_tf/centroid_coordinates", Int32MultiArray, queue_size=5)
def configure(self, robot_name):
print(rospy.get_caller_id(), ' -> configuring dvrk_arm_test for ',
robot_name)
self.arm = dvrk.arm(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)
d2r = math.pi / 180.0 # degrees to radians
r2d = 180.0 / math.pi # radians to degrees
# 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)
# 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 = raw_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")
# messages
# raw_input("To start with some initial values, you first need to \"home\" the robot. When homed, press [enter]\n")
# if arm_type == "PSM":
# raw_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":
# raw_input("Since you are calibrating an ECM, remove the endoscope and press [enter]\n")
# raw_input("The robot will make LARGE MOVEMENTS, please hit [enter] to continue once it is safe to proceed\n")
# move to the home position from the start
# this_arm.home()
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]))
time.sleep(2.0)
print(this_arm.get_current_position())
print(PyKDL.Vector(0.0, 0.05, 0.0))
this_arm.move(PyKDL.Vector(0.0, 0.0, -0.05))
print(this_arm.get_current_position())
print(this_arm.get_desired_position())
# if arm_type == "PSM":
# this_arm.move_jaw(30.0*d2r, blocking = False)
# this_arm.move_joint(numpy.array([30.0*d2r, 15.0*d2r, 0.2, 30.0*d2r, 30.0*d2r, 30.0*d2r]))
# 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]))
# time.sleep(1.0)
# # at the end, return to home position
# if arm_type == "PSM":
# this_arm.move_jaw(0.0*d2r, blocking = False)
# this_arm.move_joint(numpy.array([0.0*d2r, 0.0*d2r, 0.0, 0.0*d2r, 0.0*d2r, 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]))
## Main ##
# if __name__ == '__main__':
# if (len(sys.argv) != 4):
# print sys.argv[0] + ' requires three arguments, i.e. "scales"/"offsets", name of dVRK arm and file name. Always start with scales calibration.'
# else:
# if (sys.argv[1] == 'offsets') or (sys.argv[1] == 'scales'):
# calibration = potentiometer_calibration(sys.argv[2])
# calibration.run(sys.argv[1], sys.argv[3])
# else:
# print sys.argv[0] + ', first argument must be either scales or offsets. You must start with the scale calibration'
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 = {}
# Make sure file exists
if not os.path.exists(filename):
sys.exit("Config file \"%s\" not found" % filename)
tree = ET.parse(filename)
root = 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")
if xmlRobot.get("Name") == self._robot_name:
self._serial_number = xmlRobot.get("SN")
print(
"Successfully found robot \"%s\", serial number %s in XML file"
% (self._robot_name, self._serial_number))
robotFound = True
else:
sys.exit(
"Found robot \"%s\" while looking for \"%s\", make sure you're using the correct configuration file!"
% (xmlRobot.get("Name"), self._robot_name))
# Look for all actuators/VoltsToPosSI
xpath_search_results = root.findall("./Robot/Actuator")
for actuator in xpath_search_results:
actuatorId = int(actuator.get("ActuatorID"))
voltsToPosSI = actuator.find("./AnalogIn/VoltsToPosSI")
xmlVoltsToPosSI[actuatorId] = voltsToPosSI
# set joint limits and number of axis based on arm type, using robot name
if ("").join(
list(self._robot_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
elif self._robot_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 self._robot_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 self._robot_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
# Create the dVRK python ROS client
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 %s is not started or is not publishing the IO topics"
% self._robot_name)
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: %s" %
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: %s" % csv_file_name)
f = open(csv_file_name, 'w')
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
this_arm.move_jp(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" or arm_type == "MTM":
this_arm.move_jp(
numpy.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]))
elif arm_type == "ECM":
this_arm.move_jp(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, 'w')
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 = 2 * 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].get("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].set("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].get("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].set("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].set("Offset",
str(newOffsets[axis]))
else:
for axis in range(nb_axis):
# replace values
xmlVoltsToPosSI[axis].set("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 %s-new. Restart your dVRK application with the new file!'
% filename)
print('To copy the new file over the existing one: cp %s-new %s' %
(filename, filename))
def configure(self, robot_name):
print(rospy.get_caller_id(), ' -> configuring dvrk_arm_test for ', robot_name)
self.arm = dvrk.arm(robot_name)
def __init__(self, activate_ar=False, activate_debug_visuals=False):
#python api -- This code initializes the ros node
self.api_psm3_arm = dvrk.arm('PSM3')
#visual controller
self.dvrk_controller = dvrk_tf_module(
userId="test",
dst_path='./videos',
rig_name="pu_dvrk_cam",
activate_ar=activate_ar,
activate_debug_visuals=activate_debug_visuals)
#Ar config
self.ar_orientation = PyKDL.Rotation.Quaternion(
0.68175651, -0.27654879, 0.56096521, 0.37953506)
self.dvrk_controller.ar_orientation = self.ar_orientation
#
self.arm_kinematic = self.ArmKinematic('PSM3')
#tf
self.tf_world_psm3b = None
#frames
self.bridge = CvBridge()
self.right_frame = None
###############
##subscribers##
###############
self.psm3_cartesian_subs = rospy.Subscriber(
"/dvrk/PSM3/position_cartesian_current", PoseStamped,
self.psm3_cartesian_callback)
self.psm1_suj_subs = rospy.Subscriber("/dvrk/PSM1/io/suj_clutch", Joy,
self.setup_button_psm1_callback)
self.camera_subs = rospy.Subscriber("/pu_dvrk_cam/right/inverted",
Image, self.camera_callback)
#tf
self.tf_world_psm3b_subs = rospy.Subscriber(
"/pu_dvrk_tf/tf_world_psm3b", PoseStamped,
self.tf_world_psm3b_callback)
##############
##Publishers##
##############
self.multi_arr_pub = rospy.Publisher(
"/pu_dvrk_tf/centroid_coordinates", Int32MultiArray, queue_size=5)
#psm offset
self.fix_orientation = PyKDL.Rotation.Quaternion(
0.35131810, -0.12377625, 0.53801977, 0.75616781)
#Upload homography between pixels and robot x,y
self.homography = None
with open("./vision_modules/misc/homography_data/homography.json",
"r") as f1:
self.homography = np.array(json.load(f1))
print("Retrieved homography")
print(self.homography)
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)
#!/usr/bin/env python
import dvrk
import numpy as np
import time
#DVRK interface does not allow to instantiate multiple arms at the same time.
# Move PSM1 to the initial position
mtml = dvrk.arm('MTML')
mtml.home()
mtml_joint_pos = [ 0.16094356, 0.18617774, -0.13542208, -1.70923487, 1.67276075, -0.0769766,-1.01845961]
print('mtml current')
print(mtml.get_current_joint_position())
print('mtml next')
print(mtml_joint_pos)
mtml.move_joint(np.array([mtml_joint_pos]))
'--arm',
type=str,
required=True,
choices=['ECM', 'MTML', 'MTMR', 'PSM1', 'PSM2', 'PSM3'],
help=
'arm name corresponding to ROS topics without namespace. Use __ns:= to specify the namespace'
)
parser.add_argument(
'-i',
'--interval',
type=float,
default=0.01,
help='expected interval in seconds between messages sent by the device')
args = parser.parse_args(argv[1:]) # skip argv[0], script name
arm = dvrk.arm(arm_name=args.arm, expected_interval=args.interval)
print('starting move_jp')
# get current position
initial_joint_position = numpy.copy(arm.setpoint_jp())
amplitude = math.radians(10.0)
goal = numpy.copy(initial_joint_position)
print('--> Testing the trajectory with wait()')
start_time = time.time()
# first motion
goal[0] = initial_joint_position[0] + amplitude
arm.move_jp(goal).wait()
def configure(self, robot_name, expected_interval):
print_id('configuring dvrk_arm_test for %s' % robot_name)
self.expected_interval = expected_interval
self.arm = dvrk.arm(arm_name = robot_name,
expected_interval = expected_interval)
#!/usr/bin/env python
import dvrk
import numpy as np
import time
#DVRK interface does not allow to instantiate multiple arms at the same time.
# Move PSM1 to the initial position
mtmr = dvrk.arm('MTMR')
mtmr.home()
mtmr_joint_pos = [
-0.14427202, 0.18954733, -0.15922543, 1.67041871, 1.68164266, 0.03256702,
0.95332556
]
print('mtmr current')
print(mtmr.get_current_joint_position())
print('mtmr next')
print(mtmr_joint_pos)
mtmr.move_joint(np.array([mtmr_joint_pos]))
