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 convert_DH_to_ambf_joint(self, a, alpha, d, theta, joint_data): rot_bINa = Rotation(math.cos(theta), - math.sin(theta) * math.cos(alpha), math.sin(theta) * math.sin(alpha), math.sin(theta), math.cos(theta) * math.cos(alpha), - math.cos(theta) * math.sin(alpha), 0, math.sin(alpha), math.cos(alpha)) pos_bINa = Vector(a * math.cos(theta), a * math.sin(theta), d) for i in range(0, 3): pos_bINa[i] = round(pos_bINa[i], 3) for j in range(0, 3): rot_bINa[i, j] = round(rot_bINa[i, j], 3) trans_bINa = Frame(rot_bINa, pos_bINa) # The child pivot and axis defined the joint in child frame. So we set # the parent pivot and axis to zero and default respectively and then # invert the Trans of B in A to get Trans of A in B and set the child # pivot and axis based on that trans_aINb = trans_bINa.Inverse() print('DH: a: %s alpha: %s d: %s theta: %s', a, alpha, d, theta) print(rot_bINa) print(pos_bINa) print('------') parent_pivot = {'x': 0, 'y': 0, 'z': 0} parent_axis = {'x': 0, 'y': 0, 'z': 1} child_pivot = {'x': round(trans_aINb.p[0], 3), 'y': round(trans_aINb.p[1], 3), 'z': round(trans_aINb.p[2], 3)} child_axis = {'x': round(trans_aINb.M[0, 2], 3), 'y': round(trans_aINb.M[1, 2], 3), 'z': round(trans_aINb.M[2, 2], 3)} print(parent_pivot) print(parent_axis) print(child_pivot) print(child_axis) print('---------') joint_data['parent axis'] = parent_axis joint_data['parent pivot'] = parent_pivot joint_data['child pivot'] = child_pivot joint_data['child axis'] = child_axis
class PSM: def __init__(self, client, name): self.client = client self.name = name self.base = self.client.get_obj_handle(name + '/baselink') self.target_IK = self.client.get_obj_handle(name + '_target_ik') self.palm_joint_IK = self.client.get_obj_handle(name + '_palm_joint_ik') self.target_FK = self.client.get_obj_handle(name + '_target_fk') self.sensor = self.client.get_obj_handle(name + '/Sensor0') self.actuators = [] self.actuators.append(self.client.get_obj_handle(name + '/Actuator0')) time.sleep(0.5) self.grasped = [False, False, False] self.T_t_b_home = Frame(Rotation.RPY(3.14, 0.0, 1.57079), Vector(0.0, 0.0, -1.0)) # Transform of Base in World self._T_b_w = None # Transform of World in Base self._T_w_b = None self._base_pose_updated = False self._num_joints = 6 self._ik_solution = np.zeros([self._num_joints]) self._last_jp = np.zeros([self._num_joints]) def set_home_pose(self, pose): self.T_t_b_home = pose def is_present(self): if self.base is None: return False else: return True def get_ik_solution(self): return self._ik_solution def get_T_b_w(self): self._update_base_pose() return self._T_b_w def get_T_w_b(self): self._update_base_pose() return self._T_w_b 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 run_grasp_logic(self, jaw_angle): for i in range(len(self.actuators)): if jaw_angle <= 0.2: if self.sensor is not None: if self.sensor.is_triggered(i): sensed_obj = self.sensor.get_sensed_object(i) if sensed_obj == 'Needle' or 'Thread' in sensed_obj: if not self.grasped[i]: qualified_nane = '/ambf/env/BODY ' + sensed_obj self.actuators[i].actuate(qualified_nane) self.grasped[i] = True print('Grasping Sensed Object Names', sensed_obj) else: if self.actuators[i] is not None: self.actuators[i].deactuate() if self.grasped[i] is True: print('Releasing Grasped Object') self.grasped[i] = False # print('Releasing Actuator ', i) def move_cp(self, T_t_b): if type(T_t_b) in [np.matrix, np.array]: T_t_b = convert_mat_to_frame(T_t_b) ik_solution = compute_IK(T_t_b) self._ik_solution = enforce_limits(ik_solution) self.move_jp(self._ik_solution) def optimize_jp(self, jp): # Optimizing the tool shaft roll angle pass def move_jp(self, jp): self.base.set_joint_pos('baselink-yawlink', jp[0]) self.base.set_joint_pos('yawlink-pitchbacklink', jp[1]) self.base.set_joint_pos('pitchendlink-maininsertionlink', jp[2]) self.base.set_joint_pos('maininsertionlink-toolrolllink', jp[3]) self.base.set_joint_pos('toolrolllink-toolpitchlink', jp[4]) self.base.set_joint_pos('toolpitchlink-toolyawlink', jp[5]) def set_jaw_angle(self, jaw_angle): self.base.set_joint_pos('toolyawlink-toolgripper1link', jaw_angle) self.base.set_joint_pos('toolyawlink-toolgripper2link', jaw_angle) def measured_cp(self): pass
tf.ExtrapolationException): continue # Rotations rot_OH = Rotation.Quaternion(*rot_OH) rot_OA = Rotation.Quaternion(*rot_OA) rot_BG = Rotation.Quaternion(*rot_BG) rot_AG = Rotation.RPY(math.pi / 2, -math.pi, math.pi / 2) # Creating Frames T_OH = Frame(rot_OH, Vector(*trans_OH)) T_OA = Frame(rot_OA, Vector(*trans_OA)) T_BG = Frame(rot_BG, Vector(*trans_BG)) T_AG = Frame(rot_AG, Vector(0, 0, 0)) # Finding right transformation T_HB = T_OH.Inverse() * T_OA * T_AG * T_BG.Inverse() T_empty_p = Vector(0, 0, 0) T_empty_Q = Rotation.Quaternion(0, 0, 0, 1) T_empty = Frame(T_empty_Q, T_empty_p) # Broadcast new transformations br.sendTransform(T_HB.p, T_HB.M.GetQuaternion(), rospy.Time.now(), 'base', 'bax_head') br.sendTransform(T_HB.p, T_HB.M.GetQuaternion(), rospy.Time.now(), 'reference/base', 'bax_head') br.sendTransform(T_empty.p, T_empty.M.GetQuaternion(), rospy.Time.now(), 'world', 'base') rate.sleep()
class GeomagicDevice: # The name should include the full qualified prefix. I.e. '/Geomagic/', or '/omniR_' etc. 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 set_base_frame(self, frame): self._T_baseoffset = frame self._T_baseoffset_inverse = self._T_baseoffset.Inverse() pass def set_tip_frame(self, frame): self._T_tipoffset = frame pass def set_scale(self, scale): self._scale = scale def get_scale(self): return self._scale def pose_cb(self, msg): cur_frame = pose_msg_to_kdl_frame(msg) cur_frame.p = cur_frame.p * self._scale self.pose = self._T_baseoffset_inverse * cur_frame * self._T_tipoffset # Mark active as soon as first message comes through self._active = True pass def twist_cb(self, msg): twist = PyKDL.Twist() twist[0] = msg.linear.x twist[1] = msg.linear.y twist[2] = msg.linear.z twist[3] = msg.angular.x twist[4] = msg.angular.y twist[5] = msg.angular.z self.twist = self._T_baseoffset_inverse * twist pass def buttons_cb(self, msg): self.gripper_button_pressed = msg.white_button self.clutch_button_pressed = msg.grey_button if self.clutch_button_pressed: time_diff = rospy.Time.now() - self._button_msg_time if time_diff.to_sec() < self._switch_psm_duration.to_sec(): print('Allow PSM Switch') self.switch_psm = True self._button_msg_time = rospy.Time.now() def command_force(self, force): pass def measured_cp(self): return self.pose def measured_cv(self): return self.twist def get_jaw_angle(self): if self.gripper_button_pressed: return 0.0 else: return 0.3
class Camera: def __init__(self, client, name): self.client = client self.name = name self.camera_handle = self.client.get_obj_handle(name) time.sleep(0.5) # Transform of Base in World self._T_c_w = None # Transform of World in Base self._T_w_c = None self._pose_changed = True self._num_joints = 6 self._ik_solution = np.zeros([self._num_joints]) self._last_jp = np.zeros([self._num_joints]) def is_present(self): if self.camera_handle is None: return False else: return True def get_T_c_w(self): self._update_camera_pose() return self._T_c_w def get_T_w_c(self): self._update_camera_pose() return self._T_w_c def has_pose_changed(self): return self._pose_changed def set_pose_changed(self): self._pose_changed = True 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 move_cp(self, T_c_w): if type(T_c_w) in [np.matrix, np.array]: T_c_w = convert_mat_to_frame(T_c_w) self.camera_handle.set_pos(T_c_w.p[0], T_c_w.p[1], T_c_w.p[2]) rpy = T_c_w.M.GetRPY() self.camera_handle.set_rpy(rpy[0], rpy[1], rpy[2]) self._pose_changed = True 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 measured_cp(self): return self.get_T_c_w()
class MTM: # The name should include the full qualified prefix. I.e. '/Geomagic/', or '/omniR_' etc. def __init__(self, name): pose_sub_topic_name = name + 'position_cartesian_current' twist_topic_name = name + 'twist_body_current' joint_state_sub_topic_name = name + 'state_joint_current' gripper_topic_name = name + 'state_gripper_current' clutch_topic_name = '/dvrk/footpedals/clutch' coag_topic_name = '/dvrk/footpedals/coag' pose_pub_topic_name = name + 'set_position_cartesian' wrench_pub_topic_name = name + 'set_wrench_body' effort_pub_topic_name = name + 'set_effort_joint' self.cur_pos_msg = None self.pre_coag_pose_msg = None self._active = False self._scale = 1.0 self.pose = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0)) self.twist = PyKDL.Twist() R_off = Rotation.RPY(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.coag_button_pressed = False # Used as Position Engage Coag self.gripper_angle = 0 self._pose_sub = rospy.Subscriber(pose_sub_topic_name, PoseStamped, self.pose_cb, queue_size=1) self._state_sub = rospy.Subscriber(joint_state_sub_topic_name, JointState, self.state_cb, queue_size=1) self._gripper_sub = rospy.Subscriber(gripper_topic_name, JointState, self.gripper_cb, queue_size=1) self._twist_sub = rospy.Subscriber(twist_topic_name, TwistStamped, self.twist_cb, queue_size=1) self._clutch_button_sub = rospy.Subscriber(clutch_topic_name, Joy, self.clutch_buttons_cb, queue_size=1) self._coag_button_sub = rospy.Subscriber(coag_topic_name, Joy, self.coag_buttons_cb, queue_size=1) self._pos_pub = rospy.Publisher(pose_pub_topic_name, Pose, queue_size=1) self._wrench_pub = rospy.Publisher(wrench_pub_topic_name, Wrench, queue_size=1) self._effort_pub = rospy.Publisher(effort_pub_topic_name, JointState, queue_size=1) self.switch_psm = False self._button_msg_time = rospy.Time.now() self._switch_psm_duration = rospy.Duration(0.5) self._jp = [] self._jv = [] self._jf = [] print('Creating MTM Device Named: ', name, ' From ROS Topics') self._msg_counter = 0 def set_base_frame(self, frame): self._T_baseoffset = frame self._T_baseoffset_inverse = self._T_baseoffset.Inverse() pass def optimize_wrist_platform(self): qs = self._jp vs = self._jv Kp_4 = 0.3 Kd_4 = 0.03 lim_4 = 0.2 Kp_6 = 0.0 Kd_6 = 0.0 lim_6 = 0.01 # Limits for Wrist Pitch (Joint 5) a_lim_5 = -1.5 b_lim_5 = 1.2 c_lim_5 = 1.8 sign = 1 if a_lim_5 < qs[4] <= b_lim_5: sign = 1 elif b_lim_5 < qs[4] < c_lim_5: range = c_lim_5 - b_lim_5 normalized_val = (qs[4] - b_lim_5) / range centerd_val = normalized_val - 0.5 sign = -centerd_val * 2 print('MID VAL:', sign) # sign = 0 else: sign = -1 e = qs[5] tau_4 = Kp_4 * e * sign - Kd_4 * vs[3] tau_4 = np.clip(tau_4, -lim_4, lim_4) tau_6 = -Kp_6 * qs[5] - Kd_6 * vs[5] tau_6 = np.clip(tau_6, -lim_6, lim_6) js_cmd = [0.0] * 7 js_cmd[3] = tau_4 js_cmd[5] = tau_6 self.move_jf(js_cmd) def set_tip_frame(self, frame): self._T_tipoffset = frame pass def set_scale(self, scale): self._scale = scale def get_scale(self): return self._scale def pose_cb(self, msg): self.cur_pos_msg = msg if self.pre_coag_pose_msg is None: self.pre_coag_pose_msg = self.cur_pos_msg cur_frame = pose_msg_to_kdl_frame(msg) cur_frame.p = cur_frame.p * self._scale self.pose = self._T_baseoffset_inverse * cur_frame * self._T_tipoffset # Mark active as soon as first message comes through self._active = True pass def state_cb(self, msg): self._jp = msg.position self._jv = msg.velocity self._jf = msg.effort pass def is_active(self): return self._active def gripper_cb(self, msg): min = -1.8 max = 1.4 self.gripper_angle = msg.position[0] + min / (max - min) pass def twist_cb(self, msg): twist = PyKDL.Twist() omega = msg.twist twist[0] = omega.linear.x twist[1] = omega.linear.y twist[2] = omega.linear.z twist[3] = omega.angular.x twist[4] = omega.angular.y twist[5] = omega.angular.z self.twist = self._T_baseoffset_inverse * twist pass def clutch_buttons_cb(self, msg): self.clutch_button_pressed = msg.buttons[0] self.pre_coag_pose_msg = self.cur_pos_msg if self.clutch_button_pressed: time_diff = rospy.Time.now() - self._button_msg_time if time_diff.to_sec() < self._switch_psm_duration.to_sec(): print('Allow PSM Switch') self.switch_psm = True self._button_msg_time = rospy.Time.now() def coag_buttons_cb(self, msg): self.coag_button_pressed = msg.buttons[0] self.pre_coag_pose_msg = self.cur_pos_msg def command_force(self, force): pass def move_cp(self, pose): if type(pose) == PyKDL.Frame: pose_msg = kdl_frame_to_pose_msg(pose).pose elif type(pose) == PoseStamped: pose_msg = pose.pose else: pose_msg = pose self._pos_pub.publish(pose_msg) def move_cf(self, wrench): wrench = self._T_baseoffset_inverse * wrench wrench_msg = kdl_wrench_to_wrench_msg(wrench) self._wrench_pub.publish(wrench_msg.wrench) def move_jf(self, torques): effort_msg = vector_to_effort_msg(torques) self._effort_pub.publish(effort_msg) def measured_cp(self): return self.pose def measured_jp(self): return self._jp def measured_jf(self): return self._jf def measured_cv(self): return self.twist def get_jaw_angle(self): return self.gripper_angle
class GeomagicDevice: # The name should include the full qualified prefix. I.e. '/Geomagic/', or '/omniR_' etc. 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 set_base_frame(self, frame): self.base_frame = frame pass def set_tip_frame(self, frame): self.tip_frame = frame pass def pose_cb(self, msg): cur_frame = msg_pose_to_kdl_frame(msg) p = cur_frame.p rpy = cur_frame.M.GetRPY() # print (round(p[0], 3), round(p[1], 3), round(p[2], 3)) # print (round(rpy[0], 3), round(rpy[1], 3), round(rpy[2], 3)) # Convert the position based on the scale cur_frame.p = cur_frame.p * self._scale self.pose = self.base_frame.Inverse() * cur_frame * self.tip_frame # Mark active as soon as first message comes through self._active = True pass def buttons_cb(self, msg): self.white_button_pressed = msg.white_button self._geomagicButtonMsg = msg # If any of the two pos clutch buttons are pressed. enable clutch if self._geomagicButtonMsg.grey_button or self._externalFootPedalMsg.y == 1: self.grey_button_pressed = True else: self.grey_button_pressed = False def extra_footpedal_cb(self, msg): if msg.x == 1.0: self._engageMTM = True elif msg.x == 0.0: self._engageMTM = False self._externalFootPedalMsg = msg # If any of the two pos clutch buttons are pressed. enable clutch if self._externalFootPedalMsg.y == 1 or self._geomagicButtonMsg.grey_button: self.grey_button_pressed = True else: self.grey_button_pressed = False def command_force(self, force): pass def process_commands(self): if self._active: if self._engageMTM: self._mtm_handle.set_pose(self.pose) self._mtm_handle.set_gripper_angle(self.white_button_pressed) self._mtm_handle.set_foot_pedal(self.grey_button_pressed) self._mtm_handle.publish_status() force = self._mtm_handle.get_commanded_force() force = self.base_frame.M.Inverse() * force self._force.force.x = force[0] self._force.force.y = force[1] self._force.force.z = force[2] if self._msg_counter % 500 == 0: # print (self._force) pass if self._msg_counter >= 1000: self._msg_counter = 0 # self._force_pub.publish(self._force) self._msg_counter = self._msg_counter + 1