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 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 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 main(): _pair_one_specified = True _pair_two_specified = False rospy.init_node('geomagic_mtm_proxy_node') _geomagic_one_name = '/Geomagic/' _geomagic_two_name = '' _device_pairs = [] _mtm_one_name = 'MTMR' _mtm_two_name = 'MTML' # The publish frequency _pub_freq = 500 print('Specified Arguments') for i in range(0, len(sys.argv)): print (sys.argv[i]) if len(sys.argv) > 1: _geomagic_one_name = sys.argv[1] _pair_one_specified = True if len(sys.argv) > 2: _geomagic_two_name = sys.argv[2] _pair_two_specified = True if len(sys.argv) > 3: _pub_freq = int(sys.argv[3]) if _pair_one_specified: geomagic_one = GeomagicDevice(_geomagic_one_name, _mtm_one_name) base_frame = Frame() tip_frame = Frame() # This is important. Firs we set the orientation of the # MTM base in Geomagic Base base_frame.M = Rotation.RPY((-1.57079 - 0.6), 3.14, 0) # Then we set the offset of MTMs tip in Geomagics Base # The Geomagic Base and Tip are aligned tip_frame.M = Rotation.RPY(-3.14, 0, 1.57079) geomagic_one.set_base_frame(base_frame) geomagic_one.set_tip_frame(tip_frame) _device_pairs.append(geomagic_one) if _pair_two_specified: geomagic_two = GeomagicDevice(_geomagic_two_name, _mtm_two_name) base_frame = Frame() tip_frame = Frame() # This is important. Firs we set the orientation of the # MTM base in Geomagic Base base_frame.M = Rotation.RPY((-1.57079 - 0.6), 3.14, 0) # Then we set the offset of MTMs tip in Geomagics Base # The Geomagic Base and Tip are aligned tip_frame.M = Rotation.RPY(-3.14, 0, 1.57079) geomagic_two.set_base_frame(base_frame) geomagic_two.set_tip_frame(tip_frame) _device_pairs.append(geomagic_two) rate = rospy.Rate(_pub_freq) msg_index = 0 _start_time = rospy.get_time() while not rospy.is_shutdown(): for dev in _device_pairs: dev.process_commands() rate.sleep() msg_index = msg_index + 1 if msg_index % _pub_freq*3 == 0: # Print every 3 seconds as a flag to show that this code is alive print('Geomagic Mimic Node Alive...', round(rospy.get_time() - _start_time, 3), 'secs') if msg_index >= _pub_freq * 10: # After ten seconds, reset, no need to keep increasing this msg_index = 0