def dvrk_ecm_gc_collect(robot_name):
# create dVRK robot
robot = dvrk.ecm(robot_name)
# file to save data
now = datetime.datetime.now()
now_string = now.strftime("%Y-%m-%d-%H-%M")
csv_file_name = 'ecm-gc-' + now_string + '.csv'
print("Values will be saved in: ", csv_file_name)
f = open(csv_file_name, 'w')
writer = csv.writer(f)
# compute joint limits
d2r = math.pi / 180.0 # degrees to radians
lower_limits = [-80.0 * d2r, -40.0 * d2r, 0.005]
upper_limits = [ 80.0 * d2r, 60.0 * d2r, 0.230]
# set sampling for data
# increments = [40.0 * d2r, 40.0 * d2r, 0.10] # less samples
increments = [20.0 * d2r, 20.0 * d2r, 0.05] # more samples
directions = [1.0, 1.0, 1.0]
# start position
positions = [lower_limits[0],
lower_limits[1],
lower_limits[2]]
all_reached = False
robot.home();
while not all_reached:
next_dimension_increment = True
for index in range(3):
if next_dimension_increment:
future = positions[index] + directions[index] * increments[index]
if (future > upper_limits[index]):
directions[index] = -1.0
if index == 2:
all_reached = True
elif (future < lower_limits[index]):
directions[index] = 1.0
else:
positions[index] = future
next_dimension_increment = False
robot.move_jp(numpy.array([positions[0],
positions[1],
positions[2],
0.0])).wait()
time.sleep(1.0)
writer.writerow([robot.measured_jp()[0],
robot.measured_jp()[1],
robot.measured_jp()[2],
robot.measured_jf()[0],
robot.measured_jf()[1],
robot.measured_jf()[2]])
f.close()
robot.move_jp(numpy.array([0.0, 0.0, 0.0, 0.0])).wait()
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)
print("LEFT CAM")
print(left_camera_info)
print("RIGHT_CAM")
print(right_camera_info)
# +
psm1 = None
ecm = None
suj = None
debug_output = widgets.Output(layout={'border': '1px solid black'})
with debug_output:
global psm1, ecm
psm1 = dvrk.psm('PSM1')
ecm = dvrk.ecm('ECM')
psm2 = dvrk.psm('PSM2')
HARDCODED_ECM_POS = np.array([0.0, 0.0, 0.04, 0.0])
# -
tf_listener = tf.TransformListener()
tf_listener.getFrameStrings()
import time
time.sleep(5)
ecm.move_joint(HARDCODED_ECM_POS)
# +
pick_and_place_utils = None
def set_ecm_callback():
global current_arm
rospy.loginfo("Selected ECM for spacenav control")
current_arm = dvrk.ecm('ECM')