From 9378ab757b5f330409324f2dc1d4f819ff2ba092 Mon Sep 17 00:00:00 2001 From: Michel Breyer Date: Fri, 12 Mar 2021 18:09:05 +0100 Subject: [PATCH] Cleanup --- launch/panda_visualization.launch | 4 +-- launch/panda_visualization.rviz | 42 ++++++++++++++++++++++---- robot_sim.py | 8 +++-- run_demo.py | 4 +-- utils.py | 49 +++++++++++++------------------ 5 files changed, 68 insertions(+), 39 deletions(-) diff --git a/launch/panda_visualization.launch b/launch/panda_visualization.launch index ac1f462..74d7957 100644 --- a/launch/panda_visualization.launch +++ b/launch/panda_visualization.launch @@ -1,7 +1,7 @@ - + - + diff --git a/launch/panda_visualization.rviz b/launch/panda_visualization.rviz index 8268569..5c6909c 100644 --- a/launch/panda_visualization.rviz +++ b/launch/panda_visualization.rviz @@ -5,6 +5,8 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 + - /RobotModel1/Status1 + - /TF1/Frames1 Splitter Ratio: 0.5 Tree Height: 549 - Class: rviz/Selection @@ -61,6 +63,16 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true panda_link0: Alpha: 1 Show Axes: false @@ -106,12 +118,32 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Name: RobotModel Robot Description: robot_description TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: false + Marker Alpha: 1 + Marker Scale: 0.5 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false - Class: rviz/InteractiveMarkers Enable Transparency: true Enabled: true @@ -149,7 +181,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.841537594795227 + Distance: 1.2702323198318481 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -165,9 +197,9 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.29539811611175537 + Pitch: 0.38539746403694153 Target Frame: - Yaw: 5.128592014312744 + Yaw: 5.683584213256836 Saved: ~ Window Geometry: Displays: @@ -185,5 +217,5 @@ Window Geometry: Views: collapsed: true Width: 1200 - X: 646 - Y: 193 + X: 1045 + Y: 249 diff --git a/robot_sim.py b/robot_sim.py index 0d09f01..7c76f01 100644 --- a/robot_sim.py +++ b/robot_sim.py @@ -105,9 +105,13 @@ class SimPandaEnv(object): def _publish_state(self, event): positions, velocities = self.arm.get_state() + width = self.gripper.read() msg = JointState() msg.header.stamp = rospy.Time.now() - msg.name = ["panda_joint{}".format(i) for i in range(1, 8)] - msg.position = positions + msg.name = ["panda_joint{}".format(i) for i in range(1, 8)] + [ + "panda_finger_joint1", + "panda_finger_joint2", + ] + msg.position = np.r_[positions, 0.5 * width, 0.5 * width] msg.velocity = velocities self.state_pub.publish(msg) diff --git a/run_demo.py b/run_demo.py index 587723d..bbdd0b4 100644 --- a/run_demo.py +++ b/run_demo.py @@ -13,7 +13,7 @@ dt = 1.0 / 60.0 rospy.init_node("demo") env = SimPandaEnv(gui) -model = Model("panda_link0", "panda_link8") +model = Model("panda_link0", "panda_ee") q, dq = env.arm.get_state() x0 = model.pose(q) @@ -24,7 +24,7 @@ marker = InteractiveMarkerWrapper("target", "panda_link0", x0) # run the control loop while True: - controller.set_target(marker.get_pose()) + controller.set_target(marker.pose) q, dq = env.arm.get_state() cmd = controller.update(q, dq) env.arm.set_desired_joint_velocities(cmd) diff --git a/utils.py b/utils.py index 93dcf46..5d856c2 100644 --- a/utils.py +++ b/utils.py @@ -5,6 +5,7 @@ import PyKDL as kdl import geometry_msgs.msg from interactive_markers.interactive_marker_server import * from interactive_markers.menu_handler import * +import std_msgs.msg from visualization_msgs.msg import * @@ -20,66 +21,52 @@ class InteractiveMarkerWrapper(object): int_marker.scale = 0.2 int_marker.pose = to_pose_msg(x0) + # Attach visible sphere marker = Marker() marker.type = Marker.SPHERE - marker.scale.x = 0.05 - marker.scale.y = 0.05 - marker.scale.z = 0.05 - marker.color.r = 0.0 - marker.color.g = 0.5 - marker.color.b = 0.5 - marker.color.a = 0.6 + marker.scale = to_vector3_msg([0.05, 0.05, 0.05]) + marker.color = to_color_msg([0.0, 0.5, 0.5, 0.6]) ctrl = InteractiveMarkerControl() ctrl.always_visible = True ctrl.markers.append(marker) int_marker.controls.append(ctrl) + # Attach rotation controls ctrl = InteractiveMarkerControl() - ctrl.orientation.w = 1 - ctrl.orientation.x = 1 - ctrl.orientation.y = 0 - ctrl.orientation.z = 0 ctrl.name = "rotate_x" + ctrl.orientation = to_quat_msg(Rotation.from_quat([1, 0, 0, 1])) ctrl.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(ctrl) ctrl = InteractiveMarkerControl() - ctrl.orientation.w = 1 - ctrl.orientation.x = 0 - ctrl.orientation.y = 0 - ctrl.orientation.z = 1 ctrl.name = "rotate_y" + ctrl.orientation = to_quat_msg(Rotation.from_quat([0, 1, 0, 1])) ctrl.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(ctrl) ctrl = InteractiveMarkerControl() - ctrl.orientation.w = 1 - ctrl.orientation.x = 0 - ctrl.orientation.y = 1 - ctrl.orientation.z = 0 ctrl.name = "rotate_z" + ctrl.orientation = to_quat_msg(Rotation.from_quat([0, 0, 1, 1])) ctrl.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(ctrl) + # Attach translation controls ctrl = InteractiveMarkerControl() ctrl.name = "move_x" - ctrl.orientation.w = 1.0 - ctrl.orientation.x = 1.0 + ctrl.orientation = to_quat_msg(Rotation.from_quat([1, 0, 0, 1])) ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(ctrl) ctrl = InteractiveMarkerControl() ctrl.name = "move_y" - ctrl.orientation.w = 1.0 - ctrl.orientation.y = 1.0 + ctrl.orientation = to_quat_msg(Rotation.from_quat([0, 1, 0, 1])) ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(ctrl) ctrl = InteractiveMarkerControl() ctrl.name = "move_z" - ctrl.orientation.w = 1.0 - ctrl.orientation.z = 1.0 + ctrl.orientation = to_quat_msg(Rotation.from_quat([0, 0, 1, 1])) ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(ctrl) @@ -89,9 +76,6 @@ class InteractiveMarkerWrapper(object): def cb(self, feedback): self.pose = from_pose_msg(feedback.pose) - def get_pose(self): - return self.pose - class Transform(object): def __init__(self, rotation, translation): @@ -173,6 +157,15 @@ def kdl_to_mat(m): # ROS Conversions +def to_color_msg(color): + msg = std_msgs.msg.ColorRGBA() + msg.r = color[0] + msg.g = color[1] + msg.b = color[2] + msg.a = color[3] + return msg + + def to_point_msg(point): msg = geometry_msgs.msg.Point() msg.x = point[0]