Skip to content

Instantly share code, notes, and snippets.

@Achllle

Achllle/dq_viz.py

Last active Nov 18, 2019
Embed
What would you like to do?
visualization dual quaternions with RViz
#!/usr/bin/env python
import rospy
import copy
import numpy as np
from interactive_markers.interactive_marker_server import *
from visualization_msgs.msg import *
from dual_quaternions_ros import DualQuaternion
from geometry_msgs.msg import Vector3, Point, Pose
from std_msgs.msg import ColorRGBA
from visualization_msgs.msg import Marker, MarkerArray
class DQVizTool(object):
def __init__(self):
self.marker_pub = rospy.Publisher("visualization_marker_array", MarkerArray, queue_size=3)
self.im_server = InteractiveMarkerServer("pose_tweaker")
self.uid = 1
# make the parent marker
int_marker = InteractiveMarker()
int_marker.header.frame_id = "world"
int_marker.scale = 0.7
int_marker.pose.orientation.w = 1.
int_marker.name = "pose_tweaker"
# make the visualization of the object getting transformed
box_marker = self.make_transformation_object(pose=DualQuaternion.identity().ros_pose(),
color=ColorRGBA(0, 0, 1, 1))
# make the control marker
control = InteractiveMarkerControl()
control.always_visible = True
control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
control.orientation.w = 1.
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.always_visible = True
control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
control.orientation.w = 0.707
control.orientation.z = 0.707
int_marker.controls.append(control)
control_v = InteractiveMarkerControl()
control_v.markers.append(box_marker)
control_v.always_visible = True
int_marker.controls.append(control_v)
self.im_server.insert(int_marker, self.process_feedback)
self.im_server.applyChanges()
def process_feedback(self, feedback):
final_pose_dq = DualQuaternion.from_ros_pose(feedback.pose)
final_box = self.make_transformation_object(feedback.pose)
# screw params
l, m, theta, d = final_pose_dq.screw()
if m[0] == np.inf:
p_ = np.array([0, 0, 0])
else:
# closest point on the axis
p_ = np.cross(l, m)
tail_point = Point(*p_)
p_head = p_ + d * l
head_point = Point(*p_head)
axis_marker = Marker()
axis_marker.id = 0
axis_marker.type = Marker.ARROW
axis_marker.header.stamp = rospy.Time.now()
axis_marker.header.frame_id = "world"
axis_marker.points = [tail_point, head_point]
axis_marker.scale = Vector3(0.05, 0.1, 0.2)
axis_marker.color = ColorRGBA(1.0, 0., 0., 0.9)
# generate intermediate markers
intermediate_boxes = []
self.uid = 10
nb_boxes = 70
taus = np.linspace(0.0, 1.0, num=nb_boxes)
for tau in taus:
intermediate_dq = final_pose_dq.sclerp(DualQuaternion.identity(), final_pose_dq, tau)
intermediate_marker = self.make_transformation_object(intermediate_dq.ros_pose(),
color=ColorRGBA(0., 1., 0., np.log(tau+1)/5))
intermediate_boxes.append(intermediate_marker)
marray = MarkerArray()
marray.markers = [axis_marker, final_box] + intermediate_boxes
self.marker_pub.publish(marray)
def make_transformation_object(self, pose, color=ColorRGBA(0., 1., 0., 0.1), scale=Vector3(0.2, 0.4, 0.5)):
obj_marker = Marker()
obj_marker.id = self.uid
self.uid += 1
obj_marker.type = Marker.CUBE
obj_marker.pose = pose
obj_marker.header.stamp = rospy.Time.now()
obj_marker.header.frame_id = "world"
obj_marker.color = color
obj_marker.scale = scale
return obj_marker
if __name__ == '__main__':
rospy.init_node("dual_quaternion_viz_node", log_level=rospy.DEBUG)
viz_tool = DQVizTool()
rospy.spin()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment