Last active
November 18, 2019 09:22
-
-
Save Achllle/c06c7a9b6706d4942fdc2e198119f0a2 to your computer and use it in GitHub Desktop.
visualization dual quaternions with RViz
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/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