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 makePoses(self):
'''
产生位姿矩阵序列
'''
poses = []
points = self.makePoints()
for i in range(len(points)):
UX = Vector(points[i][0][0], points[i][0][1], points[i][0][2])
UY = Vector(points[i][1][0], points[i][1][1], points[i][1][2])
UZ = Vector(points[i][2][0], points[i][2][1], points[i][2][2])
Rot = Rotation(UX, UY, UZ)
pose = Pose()
pose.position.x = points[i][3][0]
pose.position.y = points[i][3][1]
pose.position.z = points[i][3][2]
q = Rot.GetQuaternion()
pose.orientation.x = q[0]
pose.orientation.y = q[1]
pose.orientation.z = q[2]
pose.orientation.w = q[3]
poses.append(pose)
return poses
def joy_cb(self, msg):
self._lin_vel = Vector(-msg.axes[0], -msg.axes[1], msg.axes[2])
self._ang_vel = Vector(-msg.axes[3], -msg.axes[4], msg.axes[5])
self._active = True
pass
def rot_matrix_from_vecs(vec_a, vec_b):
out = Rotation()
vec_a.Normalize()
vec_b.Normalize()
vcross = vec_a * vec_b
vdot = dot(vec_a, vec_b)
# Check if the vectors are in the same direction
if 1.0 - vdot < 0.1:
return out
# Or in the opposite direction
elif 1.0 + vdot < 0.1:
nx = Vector(1, 0, 0)
temp_dot = dot(vec_a, nx)
if -0.9 < abs(temp_dot) < 0.9:
axis = vec_a * nx
out = out.Rot(axis, 3.14)
else:
ny = Vector(0, 1, 0)
axis = vec_a * ny
out = out.Rot(axis, 3.14)
else:
skew_v = skew_mat(vcross)
out = add_mat(
add_mat(Rotation(), skew_v),
scalar_mul(skew_v * skew_v, (1 - vdot) / (vcross.Norm()**2)))
return out
def __init__(self, name, mtm_name):
pose_str = name + 'pose'
button_str = name + 'button'
force_str = name + 'force_feedback'
self._active = False
self._scale = 0.0009
self.pose = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self.base_frame = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self.tip_frame = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self.grey_button_pressed = False # Used as Position Engage Clutch
self.white_button_pressed = False # Used as Gripper Open Close Binary Angle
self._force = DeviceFeedback()
self._force.force.x = 0
self._force.force.y = 0
self._force.force.z = 0
self._force.position.x = 0
self._force.position.y = 0
self._force.position.z = 0
self._pose_sub = rospy.Subscriber(pose_str, PoseStamped, self.pose_cb, queue_size=10)
self._button_sub = rospy.Subscriber(button_str, DeviceButtonEvent, self.buttons_cb, queue_size=10)
self._force_pub = rospy.Publisher(force_str, DeviceFeedback, queue_size=1)
# External Clutch Buttons Requested by ICL
extra_footPedal_str = '/footPedal'
self._extra_pedal_cb = rospy.Subscriber(extra_footPedal_str, Vector3, self.extra_footpedal_cb, queue_size=1)
self._engageMTM = True
self._externalFootPedalMsg = Vector3()
self._geomagicButtonMsg = DeviceButtonEvent()
print('BINDING GEOMAGIC DEVICE: ', name, 'TO MOCK MTM DEVICE: ', mtm_name)
self._mtm_handle = ProxyMTM(mtm_name)
self._msg_counter = 0
def get_initial_orientation(self, robot):
vec_pose_to_goal = Vector(
self.position[0] - robot.get_position()[0],
self.position[1] - robot.get_position()[1],
0,
)
vec_pose_to_goal.Normalize()
return np.pi + (np.arccos(vec_pose_to_goal[0]) if vec_pose_to_goal[1] >
0 else -np.arccos(vec_pose_to_goal[0]))
def runOneArm(self, arm):
arm.obj_gui.App.update()
# Move the Target Position Based on the GUI
x = arm.obj_gui.x
y = arm.obj_gui.y
z = arm.obj_gui.z
ro = arm.obj_gui.ro
pi = arm.obj_gui.pi
ya = arm.obj_gui.ya
gr = arm.obj_gui.gr
arm.target.set_pos(x, y, z)
arm.target.set_rpy(ro, pi, ya)
p = arm.base.get_pos()
q = arm.base.get_rot()
P_b_w = Vector(p.x, p.y, p.z)
R_b_w = Rotation.Quaternion(q.x, q.y, q.z, q.w)
T_b_w = Frame(R_b_w, P_b_w)
p = arm.target.get_pos()
q = arm.target.get_rot()
P_t_w = Vector(p.x, p.y, p.z)
R_t_w = Rotation.Quaternion(q.x, q.y, q.z, q.w)
T_t_w = Frame(R_t_w, P_t_w)
T_t_b = T_b_w.Inverse() * T_t_w
# P_t_b = T_t_b.p
# P_t_b_scaled = P_t_b / self.psm1_scale
# T_t_b.p = P_t_b_scaled
computed_q = compute_IK(T_t_b)
# print('SETTING JOINTS: ')
# print(computed_q)
arm.base.set_joint_pos('baselink-yawlink', computed_q[0])
arm.base.set_joint_pos('yawlink-pitchbacklink', computed_q[1])
arm.base.set_joint_pos('pitchendlink-maininsertionlink', computed_q[2])
arm.base.set_joint_pos('maininsertionlink-toolrolllink', computed_q[3])
arm.base.set_joint_pos('toolrolllink-toolpitchlink', computed_q[4])
arm.base.set_joint_pos('toolpitchlink-toolyawlink', -computed_q[5])
arm.base.set_joint_pos('toolyawlink-toolgripper1link', gr)
arm.base.set_joint_pos('toolyawlink-toolgripper2link', gr)
for i in range(3):
if arm.sensor.is_triggered(i) and gr <= 0.2:
sensed_obj = arm.sensor.get_sensed_object(i)
if sensed_obj == 'Needle':
if not arm.grasped[i]:
arm.actuators[i].actuate(sensed_obj)
arm.grasped[i] = True
print('Grasping Sensed Object Names', sensed_obj)
else:
if gr > 0.2:
arm.actuators[i].deactuate()
arm.grasped[i] = False
def __init__(self):
self.ambf_data = OrderedDict()
self.ambf_data['name'] = ''
self.ambf_data['parent'] = ''
self.ambf_data['child'] = ''
self.ambf_data['parent axis'] = {'x': 0, 'y': 0.0, 'z': 1.0}
self.ambf_data['parent pivot'] = {'x': 0, 'y': 0.0, 'z': 0}
self.ambf_data['child axis'] = {'x': 0, 'y': 0.0, 'z': 1.0}
self.ambf_data['child pivot'] = {'x': 0, 'y': 0.0, 'z': 0}
self.ambf_data['joint limits'] = {'low': -1.2, 'high': 1.2}
self.ambf_data['controller'] = {'P': 1000, 'I': 0, 'D': 10}
self.origin = Vector()
self.axis = Vector(0.0, 0.0, 1.0)
def __init__(self, name):
pose_topic_name = name + 'pose'
twist_topic_name = name + 'twist'
button_topic_name = name + 'button'
force_topic_name = name + 'force_feedback'
self._active = False
self._scale = 0.0002
self.pose = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self.twist = PyKDL.Twist()
# This offset is to align the pitch with the view frame
R_off = Rotation.RPY(0.0, 0, 0)
self._T_baseoffset = Frame(R_off, Vector(0, 0, 0))
self._T_baseoffset_inverse = self._T_baseoffset.Inverse()
self._T_tipoffset = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self.clutch_button_pressed = False # Used as Position Engage Clutch
self.gripper_button_pressed = False # Used as Gripper Open Close Binary Angle
self._force = DeviceFeedback()
self._force.force.x = 0
self._force.force.y = 0
self._force.force.z = 0
self._force.position.x = 0
self._force.position.y = 0
self._force.position.z = 0
self._pose_sub = rospy.Subscriber(pose_topic_name,
PoseStamped,
self.pose_cb,
queue_size=1)
self._twist_sub = rospy.Subscriber(twist_topic_name,
Twist,
self.twist_cb,
queue_size=1)
###### Commented
self._button_sub = rospy.Subscriber(button_topic_name,
DeviceButtonEvent,
self.buttons_cb,
queue_size=1)
self._force_pub = rospy.Publisher(force_topic_name,
DeviceFeedback,
queue_size=1)
self.switch_psm = False
self._button_msg_time = rospy.Time.now()
self._switch_psm_duration = rospy.Duration(0.5)
print('Creating Geomagic Device Named: ', name, ' From ROS Topics')
self._msg_counter = 0
def move_cv(self, twist, dt):
if type(twist) in [np.array, np.ndarray]:
v = Vector(twist[0], twist[1], twist[2]) * dt
w = Vector(twist[3], twist[4], twist[5]) * dt
elif type(twist) is Twist:
v = twist.vel * dt
w = twist.rot * dt
else:
raise TypeError
T_c_w = self.get_T_c_w()
T_cmd = Frame(Rotation.RPY(w[0], w[1], w[2]), v)
self.move_cp(T_c_w * T_cmd)
pass
def attach_needle(needle, link):
error = 1000
if link is None:
print('Not a valid link, returning')
return
while error > 0.1 and not rospy.is_shutdown():
P_tINw = Vector(link.get_pos().x, link.get_pos().y, link.get_pos().z)
# R_tINw = Rotation.Quaternion(tool_yaw_link.get_rot().x,
# tool_yaw_link.get_rot().y,
# tool_yaw_link.get_rot().x,
# tool_yaw_link.get_rot().w)
R_tINw = Rotation.RPY(link.get_rpy()[0],
link.get_rpy()[1],
link.get_rpy()[2])
# we need to move the needle based on the Pose of the toolyawlink.
# The yawlink's negative Y direction faces the graspp
# position. Therefore, lets add a small offset to the P of yawlink.
y_offset = Vector(0, -0.09, 0)
P_nINw = P_tINw + R_tINw * y_offset
# If you want to rotate the needle to a certain relative orientation
# add another Rotation and multiply on the R.H.S of R_tINw in the
# Equation below.
R_nINw = R_tINw * Rotation.RPY(0, 0, 3.14)
needle.set_pos(P_nINw[0], P_nINw[1], P_nINw[2])
needle.set_rot(R_nINw.GetQuaternion())
time.sleep(0.001)
P_tINw = Vector(link.get_pos().x, link.get_pos().y, link.get_pos().z)
P_nINw = Vector(needle.get_pos().x,
needle.get_pos().y,
needle.get_pos().z)
error = (P_tINw - P_nINw).Norm()
print error
# Wait for the needle to get there
time.sleep(3)
# You should see the needle in the center of the two fingers.
# If the gripper is not already closed, you shall have to manually
# close it to grasp the needle. You should probably automate this in the testIK script.
# Don't forget to release the pose command from the needle. We can
# do so by calling:
needle.set_force(0, 0, 0)
needle.set_torque(0, 0, 0)
def __init__(self, args):
self.args = args
self.psm3 = AutonomyModule(
activate_ar=args.activate_ar,
activate_debug_visuals=args.activate_debug_visuals)
self.model, self.labels_dict = create_model()
self.centroid_module = utils.centroid_module()
#Sleep until the subscribers are ready.
time.sleep(0.20)
self.sleep_time = 1.5
#Fix landmarks
self.base_position = Vector(*[+0.04527744, -0.02624656, -0.02356771])
self.height_low = +0.07217540
self.height_medium = +0.06053191
self.height_high = self.height_low - 0.05
#Flags
self.autonomy_on = False
#############
#Subscribers#
#############
self.autonomy_flag_subs = rospy.Subscriber(
"/recording_module/autonomy_active", Bool, self.autonomy_callback)
def pose_cb(data):
global trans
p = data.pose
trans = Frame(
Rotation.Quaternion(p.orientation.x, p.orientation.y, p.orientation.z,
p.orientation.w),
Vector(p.position.x, p.position.y, p.position.z))
def compute_cam_transfrom_from_gesture(self):
if self._footpedals.cam_btn_pressed:
# Take the Norm length
init_vec = Vector(self._init_vec[0], self._init_vec[1], self._init_vec[2])
init_length = PyKDL.dot(init_vec, init_vec)
mtmr_cur_pose = self._mtmr.get_adjusted_pose()
mtml_cur_pose = self._mtml.get_adjusted_pose()
r_cur_pos = mtmr_cur_pose.p
l_cur_pos = mtml_cur_pose.p
cur_vec = l_cur_pos - r_cur_pos
cur_length = PyKDL.dot(cur_vec, cur_vec)
delta_len = cur_length - init_length
delta_rot = get_rot_mat_from_vecs(cur_vec, init_vec)
r = round(180.0/1.57079 * delta_rot.GetRPY()[0], 2)
p = round(180.0/1.57079 * delta_rot.GetRPY()[1], 2)
y = round(180.0/1.57079 * delta_rot.GetRPY()[2], 2)
# print 'ZOOM', round(delta_len, 3), 'RPY', r, p, y
else:
mtmr_init_pose = self._mtmr.get_adjusted_pose()
mtml_init_pose = self._mtml.get_adjusted_pose()
self._r_init_pos = mtmr_init_pose.p
self._l_init_pos = mtml_init_pose.p
self._init_vec = self._l_init_pos - self._r_init_pos
def compute_lin_over_thresh(err, thres):
error_sign = get_vec_sign(err)
abs_error = get_abs_vec(err)
net_thres = abs_error - thres
over_thres = get_elementwise_max(net_thres, Vector(0, 0, 0))
over_thres = element_wise_mul(error_sign, over_thres)
return over_thres
def main():
psm3 = PSM3Arm()
#Sleep until the subscribers are ready.
time.sleep(0.20)
sleep_time = 0.08
# goals = [Vector(0,0,-0.02),Vector(0,0,-0.005), Vector(0.0424, 0.0106,-0.02), Vector(0.0212,0,-0.02)]
#Corners
cols, rows = 6, 6
chessboard_scale = 1.6 / 100 #1.065 / 100
objp = np.zeros((cols * rows, 3), np.float32)
objp[:, :2] = np.mgrid[0:cols, 0:rows].T.reshape(-1, 2)
low_chessboard = objp * chessboard_scale + np.array([0, 0, -0.0010])
answer = raw_input(
"have you check the matrices and the offset are up to date? if yes write 'Y' to continue, else don't move the robot."
)
if answer != 'Y':
print("Exiting the program")
exit(0)
else:
print("Start moving arm 5 times")
for i in range(1):
print("Movement {:d}".format(i))
for i in range(low_chessboard.shape[0]):
low_goal = Vector(*low_chessboard[i, :])
high_goal = Vector(*(low_chessboard[i, :] +
np.array([0, 0, -0.02])))
print("goal {:d} ".format(i), low_goal)
print("goal {:d} ".format(i), high_goal)
print("\n")
psm3.move_psm3_to(high_goal)
time.sleep(sleep_time)
psm3.move_psm3_to(low_goal)
time.sleep(sleep_time)
try:
while not rospy.core.is_shutdown():
rospy.rostime.wallsleep(0.25)
except KeyboardInterrupt:
print("Shutting down")
def update_arm_pose(self):
gui = self.GUI
gui.App.update()
T_t_b = Frame(Rotation.RPY(gui.ro, gui.pi, gui.ya),
Vector(gui.x, gui.y, gui.z))
self.arm.move_cp(T_t_b)
self.arm.set_jaw_angle(gui.gr)
self.arm.run_grasp_logic(gui.gr)
def to_kdl_vec(urdf_vec):
# Default this vector to x axis consistent with the URDF convention
v = Vector(1.0, 0, 0)
if urdf_vec is not None:
xyz = [float(i) for i in urdf_vec.attrib['xyz'].split()]
for i in range(0, 3):
v[i] = xyz[i]
return v
def build_chain(self):
self.chain = Chain()
for e in self._config:
v = Vector(e["xyzrpy"][0], e["xyzrpy"][1], e["xyzrpy"][2])
r = Rotation.RPY(e["xyzrpy"][3], e["xyzrpy"][4], e["xyzrpy"][5])
j = Joint(e["name"], Joint.None)
f = Frame(r, v)
if e["type"] == "rotx":
axis = Vector(1.0, 0.0, 0.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis)
elif e["type"] == "roty":
axis = Vector(0.0, 1.0, 0.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis)
elif e["type"] == "rotz":
axis = Vector(0.0, 0.0, 1.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis)
elif e["type"] == "transx":
axis = Vector(1.0, 0.0, 0.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis)
elif e["type"] == "transy":
axis = Vector(0.0, 1.0, 0.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis)
elif e["type"] == "transz":
axis = Vector(0.0, 0.0, 1.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis)
self.chain.addSegment(
Segment(e["name"] + "seg", j, f, RigidBodyInertia()))
self.fksolverpos = ChainFkSolverPos_recursive(self.chain)
def __init__(self):
rospy.init_node('dvrk_slicer')
self._mtml = DvrkArm('MTML')
self._mtmr = DvrkArm('MTMR')
self._mtmr_pos_pre = None
self._mtml_pos_pre = None
self._footpedals = DvrkFootPedals()
self._cam_transform = Frame()
self._probe_transform = Frame()
self._igtl_cam_trans = igtltransform()
self._igtl_cam_trans.name = 'CameraTransform'
self._igtl_cam_trans.transform.rotation.w = 1.0
self._igtl_probe_trans = igtltransform()
self._igtl_probe_trans.name = 'ProbeTransform'
self._igtl_probe_trans.transform.rotation.w = 1.0
self._igtl_fiducial_point = igtlpoint()
self._igtl_fiducial_point.name = 'ENTRY'
self._fiducial_offset = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self._igtl_text = igtlstring()
self._igtl_cam_trans_pub = rospy.Publisher('/IGTL_TRANSFORM_OUT', igtltransform, queue_size=1)
self._igtl_probe_trans_pub = rospy.Publisher('/IGTL_TRANSFORM_OUT', igtltransform, queue_size=1)
self._igtl_fiducial_pub = rospy.Publisher('/IGTL_POINT_OUT', igtlpoint, queue_size=1)
self._igtl_status_pub = rospy.Publisher('/IGTL_TEXT_OUT', igtlstring, queue_size=1)
self._rate = rospy.Rate(120)
self._pub_msg_pairs = dict()
self._pub_msg_pairs[self._igtl_cam_trans_pub] = self._igtl_cam_trans
self._pub_msg_pairs[self._igtl_probe_trans_pub] = self._igtl_probe_trans
self._pub_msg_pairs[self._igtl_fiducial_pub] = self._igtl_fiducial_point
# self._pub_msg_pairs[self._igtl_status_pub] = self._igtl_text
self._fiducial_placement_modes = ['ENTRY', 'TARGET']
self._fiducial_placement_active_mode = 0
self._cam_init_pose_computed = False
self._r_init_pos = Vector()
self._l_init_pos = Vector()
self._init_vec = Vector()
def _update_camera_pose(self):
if self._pose_changed is True:
p = self.camera_handle.get_pos()
q = self.camera_handle.get_rot()
P_c_w = Vector(p.x, p.y, p.z)
R_c_w = Rotation.Quaternion(q.x, q.y, q.z, q.w)
self._T_c_w = Frame(R_c_w, P_c_w)
self._T_w_c = self._T_c_w.Inverse()
self._pose_changed = False
def pose_cb(self, msg):
self._pose = msg
self._frame.p = Vector(self._pose.pose.position.x,
self._pose.pose.position.y,
self._pose.pose.position.z)
self._frame.M = Rotation.Quaternion(self._pose.pose.orientation.x,
self._pose.pose.orientation.y,
self._pose.pose.orientation.z,
self._pose.pose.orientation.w)
def _update_base_pose(self):
if not self._base_pose_updated:
p = self.base.get_pos()
q = self.base.get_rot()
P_b_w = Vector(p.x, p.y, p.z)
R_b_w = Rotation.Quaternion(q.x, q.y, q.z, q.w)
self._T_b_w = Frame(R_b_w, P_b_w)
self._T_w_b = self._T_b_w.Inverse()
self._base_pose_updated = True
def convert_mat_to_frame(mat):
frame = Frame(Rotation.RPY(0, 0, 0), Vector(0, 0, 0))
for i in range(3):
for j in range(3):
frame[(i, j)] = mat[i, j]
for i in range(3):
frame.p[i] = mat[i, 3]
return frame
def test_ik(x, y, z, rx, ry, rz):
Rx = Rotation.RPY(rx, 0.0, 0.0)
Ry = Rotation.RPY(0.0, ry, 0.0)
Rz = Rotation.RPY(0.0, 0.0, rz)
tip_offset_rot = Rotation.RPY(np.pi, 0, np.pi/2)
req_rot = tip_offset_rot * Rz * Ry * Rx
req_pos = Vector(x, y, z)
T_7_0 = Frame(req_rot, req_pos)
# print "REQ POSE \n", round_transform(convert_frame_to_mat(T_7_0), 3), "\n\n--------\n\n"
compute_IK(T_7_0)
def __init__(self, leader, psm):
self.counter = 0
self.leader = leader
self.psm = psm
self.init_xyz = Vector(0.0, 0.0, -1.0)
self.init_rpy = Rotation.RPY(3.14, 0.0, 1.57079)
self.cmd_xyz = self.init_xyz
self.cmd_rpy = self.init_rpy
self.T_IK = None
def process_commands(self):
if self._active and self._obj_handle is not None:
if self._obj_handle.is_wd_expired():
self._obj_active = False
if not self._obj_active:
cur_pos = Vector(self._obj_handle.get_pos().x,
self._obj_handle.get_pos().y,
self._obj_handle.get_pos().z)
cur_rpy = Vector(self._obj_handle.get_rpy()[0],
self._obj_handle.get_rpy()[1],
self._obj_handle.get_rpy()[2])
if self._obj_handle.is_wd_expired:
self._obj_active = True
else:
cur_pos = self._cmd_pos
cur_rpy = self._cmd_rpy
rot = Rotation.RPY(0, 0, 0)
if self._ambf_cam_handle is not None:
cam_rpy = self._ambf_cam_handle.get_rpy()
rot = Rotation.RPY(cam_rpy[0], cam_rpy[1], cam_rpy[2])
self._cmd_pos = cur_pos + rot * (self._lin_vel * self._lin_scale)
self._cmd_rpy = cur_rpy + (self._ang_vel * self._ang_scale)
self._obj_handle.set_pos(self._cmd_pos[0], self._cmd_pos[1],
self._cmd_pos[2])
self._obj_handle.set_rpy(self._cmd_rpy[0], self._cmd_rpy[1],
self._cmd_rpy[2])
if self._msg_counter % 500 == 0:
pass
if self._msg_counter >= 1000:
self._msg_counter = 0
self._msg_counter = self._msg_counter + 1
def pose_to_frame(pose):
# type: (object) -> Frame
if isinstance(pose, PoseStamped):
pose = pose.pose
assert isinstance(pose, Pose)
pos = Vector(pose.position.x, pose.position.y, pose.position.z)
rot = Rotation.Quaternion(pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
frame = Frame(rot, pos)
return frame
def __init__(self, arm_name):
pose_str = '/dvrk/' + arm_name + '/position_cartesian_current'
wrench_str = '/dvrk/' + arm_name + '/set_wrench_body'
gripper_str = '/dvrk/' + arm_name + '/state_gripper_current'
status_str = '/dvrk/' + arm_name + '/status'
self._mtm_arm_type = None
if arm_name == 'MTMR':
self._mtm_arm_type = 0
elif arm_name == 'MTML':
self._mtm_arm_type = 1
else:
print('SPECIFIED ARM: ', arm_name)
print('WARNING, MTM ARM TYPE NOT UNDERSTOOD, SHOULD BE MTMR or MTML')
pass
self.pose = Frame()
self.pose.p = Vector(0, 0, 0)
self.pose.M = Rotation.Quaternion(0, 0, 0, 1)
self.buttons = 0
self._commanded_force = Vector(0, 0, 0)
self._gripper_min_angle = -3.16
self._gripper_max_angle = 1.2
self._gripper_angle = JointState()
self._gripper_angle.position.append(0)
self._pose_pub = rospy.Publisher(pose_str, PoseStamped, queue_size=1)
self._gripper_pub = rospy.Publisher(gripper_str, JointState, queue_size=1)
self._status_pub = rospy.Publisher(status_str, Empty, queue_size=1)
self._force_sub = rospy.Subscriber(wrench_str, Wrench, self.force_cb, queue_size=10)
self._foot_pedal = ProxyButtons()
pass
def ar_animation(self, init_pt, end_pt, n=40, animation_sleep=0.040):
timeout = 14
init_time = time.time()
trajectory = self.generate_trajectory(init_pt, end_pt, n=n)
for i in range(n):
new_position = Vector(*list(trajectory[:, i]))
self.set_activate_ar(
PyKDL.Frame(self.fix_orientation, new_position))
time.sleep(animation_sleep)
if time.time() - init_time > timeout:
break
