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