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 _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 _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 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 to_kdl_frame(urdf_frame): f = Frame() if urdf_frame is not None: if 'xyz' in urdf_frame.attrib: xyz = [float(i) for i in urdf_frame.attrib['xyz'].split()] for i in range(0, 3): f.p[i] = xyz[i] if 'rpy' in urdf_frame.attrib: rpy = [float(i) for i in urdf_frame.attrib['rpy'].split()] f.M = Rotation.RPY(rpy[0], rpy[1], rpy[2]) return f
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, 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 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
def fk(self, chain_state, link_num=-2): if link_num < 0: link_num -= 1 link_num += 1 if link_num < 0: link_num = self._M pos_scaled = [ cur_pos * cur_gearing for cur_pos, cur_gearing in zip( chain_state.actual.positions, self._gearing) ] #pos_scaled = JntArray(pos_scaled) jnt_states = JntArray(len(pos_scaled)) for i, s in enumerate(pos_scaled): jnt_states[i] = s self.build_chain() F1 = Frame() assert (0 == self.fksolverpos.JntToCart(jnt_states, F1, link_num)) out = [] for i in range(3): line = [] for j in range(3): line.append(F1.M[i, j]) line.append(F1.p[i]) out.append(line) out.append([0, 0, 0, 1]) out = matrix(out) return out
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 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 vel_calc_func(start_mat, end_mat, corrected_average_interval): # Create rotation matrices start_r = start_mat.M end_r = end_mat.M # Transform matrices using inverse for correct orientation temp = start_mat.M.Inverse() * end_mat.M temp_mat = Frame(Rotation(temp)) ang, temp_axis = Rotation.GetRotAngle(temp) o = start_mat.M * temp_axis p_start_vec = start_mat.p p_end_vec = end_mat.p # print "p vectors ", p_end_vec, p_start_vec delta_x = p_end_vec[0] - p_start_vec[0] print "delta x is ", delta_x delta_y = p_end_vec[1] - p_start_vec[1] delta_z = p_end_vec[2] - p_start_vec[2] # Assign values to a Vector3 element twist_vel = geometry_msgs.msg.Vector3() twist_vel.x = delta_x / corrected_average_interval.to_sec() twist_vel.y = delta_y / corrected_average_interval.to_sec() twist_vel.z = delta_z / corrected_average_interval.to_sec() # twist_rot = geometry_msgs.msg.Vector3() twist_rot = o * (ang / corrected_average_interval.to_sec()) print "twist vel is ", twist_vel print "twist rot is ", twist_rot[2] return twist_vel, twist_rot
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 __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 __init__(self): # Get the ~private namespace parameters from command li self.poseArray = PoseArray() self.waypointArray = PoseArray() self.filename = "/home/shart/marker_trajectories/hose/hose_waypoints_1.txt" self.pathPub = rospy.Publisher("/path_publisher/path", Path) self.posePub = rospy.Publisher("/path_publisher/poseArray", PoseArray) self.waypointPub = rospy.Publisher("/path_publisher/waypointArray", PoseArray) self.num_points = 10 self.frameOffset = Frame()
def test_roundtrip(self): c = Frame() d = pm.fromMsg(pm.toMsg(c)) self.assertEqual(repr(c), repr(d)) d = pm.fromMatrix(pm.toMatrix(c)) self.assertEqual(repr(c), repr(d)) d = pm.fromTf(pm.toTf(c)) self.assertEqual(repr(c), repr(d))
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): self.ambf_data = OrderedDict() self.ambf_data['name'] = "" self.ambf_data['mesh'] = "" self.ambf_data['mass'] = 0.0 self.ambf_data['inertia'] = {'ix': 0.0, 'iy': 0.0, 'iz': 0.0} self.ambf_data['scale'] = 1.0 self.ambf_data['location'] = { 'position': { 'x': 0, 'y': 0, 'z': 0 }, 'orientation': { 'r': 0, 'p': 0, 'y': 0 } } self.ambf_data['inertial offset'] = { 'position': { 'x': 0, 'y': 0, 'z': 0 }, 'orientation': { 'r': 0, 'p': 0, 'y': 0 } } self.ambf_data['color'] = 'random' self.inertial_offset = Frame() self.visual_offset = Frame() self.collision_offset = Frame() self.parent = None self.children = [] # If the urdf link has not visual geometry, it means that its purely for offsets and hence # mark it as kinematic self.is_kinematic = False
def update_arm_pose(self): twist = self.leader.measured_cv() if not self.leader.gripper_button_pressed: self.cmd_xyz = self.cmd_xyz + twist.vel * 0.00005 rot_offset_correction = Rotation.RPY(0.0, 0.0, 0.0) self.cmd_rpy = self.leader.measured_cp().M * self.init_rpy self.T_IK = Frame(self.cmd_rpy, self.cmd_xyz) self.psm.move_cp(self.T_IK) self.psm.set_jaw_angle(self.leader.get_jaw_angle()) self.psm.run_grasp_logic(self.leader.get_jaw_angle())
def compute_effector_position(self, msg): positions = JntArray(3) chain = Chain() kdl_frame = Frame() d = 0 theta = 0 order = [1, 2, 0] for i in order: joint = self.params[i] a = joint["a"] d = joint["d"] alpha = joint["alpha"] theta = joint["theta"] frame = kdl_frame.DH(a, alpha, d, theta) chain.addSegment(Segment(Joint(Joint.RotZ), frame)) positions[i] = msg.position[i] fk_solver = ChainFkSolverPos_recursive(chain) result = Frame() fk_solver.JntToCart(positions, result) kdl_pose = PoseStamped() kdl_pose.header.frame_id = 'base_link' kdl_pose.header.stamp = rospy.Time.now() kdl_pose.pose.position.x = result.p[0] kdl_pose.pose.position.y = result.p[1] kdl_pose.pose.position.z = result.p[2] quat = result.M.GetQuaternion() kdl_pose.pose.orientation.x = quat[0] kdl_pose.pose.orientation.y = quat[1] kdl_pose.pose.orientation.z = quat[2] kdl_pose.pose.orientation.w = quat[3] return kdl_pose
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 update_arm_pose(self): self.update_T_b_c() twist = self.leader.measured_cv() self.cmd_xyz = self.active_psm.T_t_b_home.p if not self.leader.clutch_button_pressed: delta_t = self._T_c_b.M * twist.vel * 0.002 self.cmd_xyz = self.cmd_xyz + delta_t self.active_psm.T_t_b_home.p = self.cmd_xyz self.cmd_rpy = self._T_c_b.M * self.leader.measured_cp().M * Rotation.RPY(np.pi, 0, np.pi / 2) self.T_IK = Frame(self.cmd_rpy, self.cmd_xyz) self.active_psm.move_cp(self.T_IK) self.active_psm.set_jaw_angle(self.leader.get_jaw_angle()) self.active_psm.run_grasp_logic(self.leader.get_jaw_angle())
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 pose_msg_to_kdl_frame(msg_pose): pose = msg_pose.transform f = Frame() f.p[0] = pose.translation.x f.p[1] = pose.translation.y f.p[2] = pose.translation.z f.M = Rotation.Quaternion(pose.rotation.x, pose.rotation.y, pose.rotation.z, pose.rotation.w) return f
def pose_msg_to_kdl_frame(msg_pose): pose = msg_pose.pose f = Frame() f.p[0] = pose.position.x f.p[1] = pose.position.y f.p[2] = pose.position.z f.M = Rotation.Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) return f
def hydra_msg_to_kdl_frame(msg_hydra): pose = msg_hydra.paddles[0].transform f = Frame() f.p[0] = 10*pose.translation.x f.p[1] = 10*pose.translation.y f.p[2] = 10*pose.translation.z f.M = Rotation.Quaternion(pose.rotation.x, pose.rotation.y, pose.rotation.z, pose.rotation.w) return f
def update_arm_pose(self): self.update_T_b_c() if self.leader.coag_button_pressed or self.leader.clutch_button_pressed: self.leader.optimize_wrist_platform() else: if self.leader.is_active(): self.leader.move_cp(self.leader.pre_coag_pose_msg) twist = self.leader.measured_cv() * 0.035 self.cmd_xyz = self.active_psm.T_t_b_home.p if not self.leader.clutch_button_pressed: delta_t = self._T_c_b.M * twist.vel self.cmd_xyz = self.cmd_xyz + delta_t self.active_psm.T_t_b_home.p = self.cmd_xyz self.cmd_rpy = self._T_c_b.M * self.leader.measured_cp().M self.T_IK = Frame(self.cmd_rpy, self.cmd_xyz) self.active_psm.move_cp(self.T_IK) self.active_psm.set_jaw_angle(self.leader.get_jaw_angle()) self.active_psm.run_grasp_logic(self.leader.get_jaw_angle())
def update_visual_markers(self): # Move the Target Position Based on the GUI if self.arm.target_IK is not None: gui = self.GUI T_ik_w = self.arm.get_T_b_w() * Frame( Rotation.RPY(gui.ro, gui.pi, gui.ya), Vector(gui.x, gui.y, gui.z)) self.arm.target_IK.set_pos(T_ik_w.p[0], T_ik_w.p[1], T_ik_w.p[2]) self.arm.target_IK.set_rpy(T_ik_w.M.GetRPY()[0], T_ik_w.M.GetRPY()[1], T_ik_w.M.GetRPY()[2]) if self.arm.target_FK is not None: ik_solution = self.arm.get_ik_solution() ik_solution = np.append(ik_solution, 0) T_t_b = convert_mat_to_frame(compute_FK(ik_solution)) T_t_w = self.arm.get_T_b_w() * T_t_b self.arm.target_FK.set_pos(T_t_w.p[0], T_t_w.p[1], T_t_w.p[2]) self.arm.target_FK.set_rpy(T_t_w.M.GetRPY()[0], T_t_w.M.GetRPY()[1], T_t_w.M.GetRPY()[2])
def __init__(self, arm_name): self._arm_name = arm_name mtml = mtm('MTML') mtmr = mtm('MTMR') self._base_offset = Frame() self._tip_offset = Frame() self._adjusted_pose = Frame() self._adjusted_pose_pre = None self._scale = 1000 if arm_name == 'MTMR': self._base_offset = Frame(Rotation.RPY(3.0397, -1.0422, -1.477115), Vector(-0.182, -0.016, -0.262)) self._tip_offset = Frame(Rotation.RPY(-1.57079, 0 , -1.57079), Vector(0,0,0)) elif arm_name == 'MTML': self._base_offset = Frame(Rotation.RPY(3.0397, -1.0422, -1.477115), Vector(0.182, -0.016, -0.262)) self._tip_offset = Frame(Rotation.RPY(-1.57079, 0 , -1.57079), Vector(0, 0, 0)) else: assert (arm_name == 'MTML' and arm_name == 'MTMR') mtml.home() mtmr.home() mtml.set_wrench_body_orientation_absolute(True) mtmr.set_wrench_body_orientation_absolute(True) mtml.set_wrench_body_force((0, 0, 0)) mtmr.set_wrench_body_force((0, 0, 0)) self._pose = PoseStamped() self._frame = Frame() self._pose_sub = rospy.Subscriber('/dvrk/' + arm_name + '/position_cartesian_current', PoseStamped, self.pose_cb)
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])