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 main(): global openhmd_state, occulus_state, openhmd_state_valid, occulus_state_valid rospy.init_node('occulus_view') openhmd_sub = rospy.Subscriber("/openhmd/pose", Pose, openhmd_cb) occulus_sub = rospy.Subscriber("/ambf/env/openhmd/State", ObjectState, occulus_cb, queue_size=1) occulus_pub = rospy.Publisher("/ambf/env/openhmd/Command", ObjectCmd, queue_size=1) rate = rospy.Rate(60) counter = 0 start = rospy.get_time() _first = True openhmd_initial_rot = Rotation() occulus_initial_rot = Rotation() R_pre = Rotation() R_aInr_offset = Rotation().RPY(0, -1.57079, -1.57079) # open while not rospy.is_shutdown(): if openhmd_state_valid and occulus_state_valid: if _first: _first = False openhmd_initial_rot = Rotation.Quaternion(openhmd_state.orientation.x, openhmd_state.orientation.y, openhmd_state.orientation.z, openhmd_state.orientation.w) occulus_initial_rot = Rotation.Quaternion(occulus_state.pose.orientation.x, occulus_state.pose.orientation.y, occulus_state.pose.orientation.z, occulus_state.pose.orientation.w) R_pre = openhmd_initial_rot * R_aInr_offset * occulus_initial_rot.Inverse() else: occulus_cmd.pose.position = occulus_state.pose.position openhmd_rot = Rotation.Quaternion(openhmd_state.orientation.x, openhmd_state.orientation.y, openhmd_state.orientation.z, openhmd_state.orientation.w) delta_rot = R_pre.Inverse() * openhmd_rot * R_aInr_offset # delta_rot = openhmd_rot occulus_cmd.pose.orientation.x = delta_rot.GetQuaternion()[0] occulus_cmd.pose.orientation.y = delta_rot.GetQuaternion()[1] occulus_cmd.pose.orientation.z = delta_rot.GetQuaternion()[2] occulus_cmd.pose.orientation.w = delta_rot.GetQuaternion()[3] occulus_cmd.enable_position_controller = True occulus_pub.publish(occulus_cmd) counter = counter + 1 if counter % 60 == 0: print "- Publishing Occulus Pose ", format( round(rospy.get_time() - start, 3)), 's' rate.sleep()
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 _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 _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 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 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 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 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 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 _getOrientation(self, data): q = Quaternion() if data: global hydro if hydro: if 'theta' in data: # Assume Yaw Orientation orientation = Rotation.RotZ(data.get('theta')) elif any(k in ['roll', 'pitch', 'yaw'] for k in data.iterkeys()): # Assume RPY Orientation orientation = Rotation.RPY(data.get('roll', 0), data.get('pitch', 0), data.get('yaw', 0)) elif any(k in ['w', 'x', 'y', 'z'] for k in data.iterkeys()): orientation = Rotation.Quaternion(data.get('x', 0), data.get('y', 0), data.get('z', 0), data.get('w', 0)) else: orientation = Rotation() orientation = orientation.GetQuaternion() else: if 'theta' in data: # Assume Yaw Orientation orientation = quaternion_from_euler( 0, 0, data.get('theta')) elif any(k in ['roll', 'pitch', 'yaw'] for k in data.iterkeys()): # Assume RPY Orientation orientation = quaternion_from_euler( data.get('roll', 0), data.get('pitch', 0), data.get('yaw', 0)) elif any(k in ['w', 'x', 'y', 'z'] for k in data.iterkeys()): orientation = (data.get('x', 0), data.get('y', 0), data.get('z', 0), data.get('w', 0)) else: orientation = (0, 0, 0, 0) q.x, q.y, q.z, q.w = orientation return q
def odom_callback(msg): odom_pose_x = msg.pose.pose.position.x odom_pose_y = msg.pose.pose.position.y qx = msg.pose.pose.orientation.x qy = msg.pose.pose.orientation.y qz = msg.pose.pose.orientation.z qw = msg.pose.pose.orientation.w r = Rotation.Quaternion(qx, qy, qz, qw) (roll, pitch, odom_pose_r) = r.GetRPY() world.shared_resource_lock.acquire() world.odom_pose = [odom_pose_x, odom_pose_y, odom_pose_r] world.move_speed = [ msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.angular.z ] if world.move_speed[0] < 0.01 and world.move_speed[ 1] < 0.01 and world.move_speed[2] < 0.01: world.rob_is_stop = True else: world.rob_is_stop = False world.odom_pose_time = msg.header.stamp.to_sec() world.shared_resource_lock.release()
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
# #High point # psm3.move(PyKDL.Frame(fix_orientation, Vector(0.20207338, -0.08238340, 0.04522971))) # time.sleep(sleep_time) # #Point 3 # psm3.move(PyKDL.Frame(fix_orientation, Vector(0.25669699, -0.17719280, 0.05745484))) # time.sleep(sleep_time) # #High point # psm3.move(PyKDL.Frame(fix_orientation, Vector(0.20207338, -0.08238340, 0.04522971))) # time.sleep(sleep_time) ## Movements with full frames (position + orientation) fix_orientation = Rotation.Quaternion( 0.26282424, -0.12377510, 0.49116142, 0.82119644, ) #Point 1 psm3.move( PyKDL.Frame(fix_orientation, Vector(0.22415668, -0.16345158, 0.04202124))) time.sleep(sleep_time) # #Point 2 # psm3.move(PyKDL.Frame(fix_orientation, Vector(0.21758554, -0.16898800, 0.04562245))) # time.sleep(sleep_time) # #Point 3 # psm3.move(PyKDL.Frame(fix_orientation, Vector(0.20161770, -0.09351339, 0.05061632))) # time.sleep(sleep_time) #dmove and Rotation movements didn't seem to work very well
# Retrieve start and end velocity and angular velocity values of tracking frame wrt reference frame (lin_twist_start, ang_twist_start) = listener.lookupTwist(reference_frame, tracking_frame, start_time, rospy.Duration(0.1)) (lin_twist_end, ang_twist_end) = listener.lookupTwist(reference_frame, tracking_frame, end_time, rospy.Duration(0.1)) # print "start quat is ", start_quat # Create matrices for calculation of velocity which can be given into the custom function start_mat_vel = Frame( Rotation.Quaternion(start_quat[0], start_quat[1], start_quat[2], start_quat[3]), Vector(start_pos[0], start_pos[1], start_pos[2])) end_mat_vel = Frame( Rotation.Quaternion(end_quat[0], end_quat[1], end_quat[2], end_quat[3]), Vector(end_pos[0], end_pos[1], end_pos[2])) # Create matrices for calculation of acceleration which can be given into the custom function start_mat_acc = Frame( Rotation.Quaternion(start_quat[0], start_quat[1], start_quat[2], start_quat[3]), Vector(lin_twist_start[0], lin_twist_start[1], lin_twist_start[2])) end_mat_acc = Frame( Rotation.Quaternion(end_quat[0], end_quat[1], end_quat[2], end_quat[3]),
def get_box_rot(): br = box.get_rot() return Rotation.Quaternion(br.x, br.y, br.z, br.w)
def cs_cb(self, msg): self._cart_state = msg self._ee_rot = Rotation.Quaternion(msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w)
cur_time = rospy.Time.now().to_sec() torque = Vector(0, 0, 0) last_torque = Vector(0, 0, 0) d_torque = Vector(0, 0, 0) # Main Loop while not rospy.is_shutdown(): App.update() last_time = cur_time cur_time = rospy.Time.now().to_sec() dt = cur_time - last_time if dt < 0.001: dt = 0.001 if active: cur_rot = Rotation.Quaternion(state_msg.pose.orientation.x, state_msg.pose.orientation.y, state_msg.pose.orientation.z, state_msg.pose.orientation.w) cur_pos = Vector(state_msg.pose.position.x, state_msg.pose.position.y, state_msg.pose.position.z) cmd_pos = Vector(PU.x, PU.y, PU.z) cmd_rot = Rotation.RPY(PU.roll, PU.pitch, PU.yaw) last_pos = cur_pos last_delta_pos = delta_pos delta_pos = cmd_pos - cur_pos delta_delta_pos = (delta_pos - last_delta_pos) / dt # print delta_pos, last_delta_pos # print (D_lin * delta_delta_pos) / dtp force = PU.K_lin * delta_pos + PU.D_lin * delta_delta_pos
""" This script broadcasts transformations from Optitrack to Baxter coordinate frame. Values for T_HB are constant. How to get those numbers is explained in README.md """ import rospy import tf from PyKDL import Vector, Frame, Rotation if __name__ == '__main__': rospy.init_node('BaxterTF') listener = tf.TransformListener() br = tf.TransformBroadcaster() rate = rospy.Rate(50) while not rospy.is_shutdown(): T_HB_p = Vector(-0.0924028561603, -0.0172428611168, -0.944187986747) T_HB_Q = Rotation.Quaternion(-0.00993130, 0.00080579, -0.00441977, 0.99994059) # qx,qy,qz,qw T_HB = Frame(T_HB_Q, T_HB_p) 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) # Sending 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()
try: (trans_OH, rot_OH) = listener.lookupTransform('/optitrack', '/bax_head', rospy.Time(0)) (trans_OA, rot_OA) = listener.lookupTransform('/optitrack', '/bax_arm', rospy.Time(0)) (trans_BG, rot_BG) = listener.lookupTransform('/base', '/left_gripper_base', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, 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)
rospy.init_node('baxter_optitrack_transformer') listener = tf.TransformListener() br = tf.TransformBroadcaster() rate = rospy.Rate(50.0) while not rospy.is_shutdown(): # Transformation ------------------------------------------------------------- #T_GB_p=Vector(0.0707,0.0187,-0.8593) # project #T_GB_Q=Rotation.Quaternion(-0.004002,-0.004752,0.999994,-0.000251) #T_GB=Frame(T_GB_Q,T_GB_p) [0.101118 <-0.573142, 0.049050, 0.811713>] (0.421538700623461, 0.5060896478392994, 0.5964256398717394, -0.45875358127230415) T_GB_p = Vector(-0.0157712999511, -0.835218066682, -0.0674771192051) #INKA T_GB_Q = Rotation.Quaternion(-0.484148, -0.486831, -0.522866, 0.505181) # qz,qx,qy,qw INKA #T_GB_p=Vector(0.0366584397092, -0.839661563598, -0.202769519381) #JA #T_GB_Q=Rotation.Quaternion(0.421538700623461, 0.5060896478392994, 0.5964256398717394, -0.45875358127230415) # qz,qx,qy,qw JA T_GB = Frame(T_GB_Q, T_GB_p) 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) Rotx_p = Vector(0, 0, 0) Rotx_Q = Rotation.Quaternion(0.70682518, 0, 0, 0.70682518) # x za 90 Rotx = Frame(Rotx_Q, Rotx_p) Roty_p = Vector(0, 0, 0) Roty_Q = Rotation.Quaternion(0, 0.70682518, 0, 0.70738827) # y za 90