Skip to content

Commit

Permalink
New method using interactive markers (over standard markers) for dyna…
Browse files Browse the repository at this point in the history 
…mic objects

Confirmed added shapes (sphere, cylinder, cuboid) can be interactively moved in x, y and z through RVIZ
Confirmed moved shapes update collision position and collision checking working with changing poses
Noted that rotation does not seem to update collision shape (but actual shape in swift updated). This is to be investivated
TODO: confirm efficiency of operation using new method
  • Loading branch information
@DasGuna
DasGuna committed Dec 6, 2023
1 parent 7dfffaf commit ac36e18
Show file tree
Hide file tree
Showing 2 changed files with 176 additions and 11 deletions.
178 changes: 168 additions & 10 deletions armer/robots/ROSRobot.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,10 +43,11 @@

# TESTING NEW IMPORTS FOR TRAJ DISPLAY (MOVEIT)
from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory
from moveit_msgs.msg import RobotState, AttachedCollisionObject, CollisionObject, RobotTrajectory, DisplayTrajectory
from moveit_msgs.msg import DisplayTrajectory

# TESTING NEW IMPORTS FOR DYNAMIC OBJECT MARKER DISPLAY (RVIZ)
from visualization_msgs.msg import Marker, MarkerArray
from visualization_msgs.msg import Marker, MarkerArray, InteractiveMarker, InteractiveMarkerControl
from interactive_markers.interactive_marker_server import InteractiveMarkerServer, InteractiveMarkerFeedback

# TESTING CONTROLLER SWITCHING FOR NEW TRAJECTORY CONTROL
from controller_manager_msgs.srv import SwitchController
Expand Down Expand Up @@ -77,6 +78,7 @@ class DynamicCollisionObj:
id: int = 0
pose: Pose = Pose()
is_added: bool = False
marker_created: bool = False

class ROSRobot(rtb.Robot):
"""
Expand Down Expand Up @@ -216,6 +218,9 @@ def __init__(self,
# of a collision situation, based on previously executed joint states that were safe
self.q_safe_window = np.zeros([200,len(self.q)])
self.q_safe_window_p = 0

# Setup interactive marker (for use with RVIZ)
self.interactive_marker_server = InteractiveMarkerServer("armer_interactive_objects")
# --- COLLISION HANDLING SETUP SECTION END--- #

self.joint_indexes = []
Expand Down Expand Up @@ -1858,10 +1863,10 @@ def update_collision_check_window(self, req: EmptyRequest) -> EmptyResponse:
NOTE: check failure modes (i.e., not in link dict, etc.)
"""
pass

def add_collision_obj_cb(self, req: AddCollisionObjectRequest) -> AddCollisionObjectResponse:
"""
TODO: Implement a way to add to existing collision dictionary at runtime
Adds a collision primative (sphere, cylinder, cuboid) to existing collision dictionary at runtime
Expected input:
-> name (string): to define the key within the collision dictionary
-> type (string): to define basic primatives (as part of the sg.Shape class)
Expand Down Expand Up @@ -1915,6 +1920,7 @@ def add_collision_obj_cb(self, req: AddCollisionObjectRequest) -> AddCollisionOb

# Test adding to collision dictionary for checking
# NOTE: perform error checking on name key (unless a replace flag was set)
interactive_marker_handle = True
if req.name in self.collision_dict.keys() and not req.overwrite:
rospy.logerr(f"Requested name [{req.name}] already exists in collision list and not asked to overwrite [{req.overwrite}]")
return AddCollisionObjectResponse(success=False)
Expand All @@ -1924,6 +1930,7 @@ def add_collision_obj_cb(self, req: AddCollisionObjectRequest) -> AddCollisionOb
rospy.logwarn(f"[NOT YET FULLY IMPLEMENTED] Here attempting to remove existing shape")
removed_obj = self.dynamic_collision_dict.pop(req.name)
self.dynamic_collision_removal_dict[req.name] = removed_obj
interactive_marker_handle = False

# Create a Dynamic Collision Object and add the configured shape to the scene
dynamic_obj = DynamicCollisionObj(shape=shape, key=req.name, pose=req.pose, is_added=False)
Expand All @@ -1942,6 +1949,11 @@ def add_collision_obj_cb(self, req: AddCollisionObjectRequest) -> AddCollisionOb
self.characterise_collision_overlaps()

# print(f"Current collision objects: {self.collision_obj_list}")
# Add the shape as an interactive marker for easy re-updating
# NOTE: wait for shape to be added to backend
if interactive_marker_handle:
rospy.sleep(rospy.Duration(secs=1))
self.interactive_marker_creation()

return AddCollisionObjectResponse(success=True)

Expand Down Expand Up @@ -1989,6 +2001,7 @@ def collision_recover_cb(self, req: EmptyRequest) -> EmptyResponse:
self.moving = False

return EmptyResponse()

# --------------------------------------------------------------------- #
# --------- Collision and Singularity Checking Methods ---------------- #
# --------------------------------------------------------------------- #
Expand Down Expand Up @@ -2752,6 +2765,7 @@ def general_executor(self, q, pose: Pose = None, collision_ignore: bool =False,

# Send empty data at end to clear visual trajectory
marker_traj = Marker()
marker_traj.header.frame_id = self.base_link.name
marker_traj.points = []
self.display_traj_publisher.publish(marker_traj)

Expand Down Expand Up @@ -3105,9 +3119,150 @@ def workspace_display(self):
NOTE: should only run this once every time a new workspace/change to workspace has been done
"""
pass

def int_marker_cb(self, feedback):
"""
On mouse release (valid click event) this method updates the corresponding
object pose in the collision dictionary
"""
# Update pose of shape on a mouse release event
if feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
# Update collision dict shape information
shape = self.collision_dict[feedback.marker_name] if feedback.marker_name in self.collision_dict else None
if shape == None or len(shape) > 1 or len(shape) <= 0:
rospy.logerr(f"[INTERACTIVE MARKER UPDATE] -> Could not access [{feedback.marker_name}]")
else:
pose_se3 = SE3(feedback.pose.position.x,
feedback.pose.position.y,
feedback.pose.position.z) * UnitQuaternion(feedback.pose.orientation.w, [
feedback.pose.orientation.x,
feedback.pose.orientation.y,
feedback.pose.orientation.z]).SE3()
shape[0].T = pose_se3.A

rospy.loginfo(f"[INTERACTIVE MARKER UPDATE] -> Obj {feedback.marker_name} updated to pose: {feedback.pose}")
self.interactive_marker_server.applyChanges()

def normalizeQuaternion( self, quaternion_msg ):
norm = quaternion_msg.x**2 + quaternion_msg.y**2 + quaternion_msg.z**2 + quaternion_msg.w**2
s = norm**(-0.5)
quaternion_msg.x *= s
quaternion_msg.y *= s
quaternion_msg.z *= s
quaternion_msg.w *= s

def interactive_marker_creation(self):
"""
Publishes interactive marker versions of added shape objects
Currently handles only:
- Sphere
- Cylinder
- Cuboid
"""
dyn_col_dict_cp = self.dynamic_collision_dict.copy()
for obj in list(dyn_col_dict_cp.values()):
# Ensure object has been successfully added to backend first
if obj.is_added and not obj.marker_created:
print(f"Here")
# Default interactive marker setup
marker = Marker()
if obj.shape.stype == 'sphere':
marker.type = Marker.SPHERE

# Expects diameter (m)
marker.scale.x = obj.shape.radius * 2
marker.scale.y = obj.shape.radius * 2
marker.scale.z = obj.shape.radius * 2
elif obj.shape.stype == 'cylinder':
marker.type = Marker.CYLINDER

# Expects diameter (m)
marker.scale.x = obj.shape.radius * 2
marker.scale.y = obj.shape.radius * 2
marker.scale.z = obj.shape.length
elif obj.shape.stype == 'cuboid':
marker.type = Marker.CUBE

# Expects diameter (m)
marker.scale.x = obj.shape.scale[0]
marker.scale.y = obj.shape.scale[1]
marker.scale.z = obj.shape.scale[2]
else:
break

int_marker = InteractiveMarker()
int_marker.header.frame_id = self.base_link.name
int_marker.scale = np.max([marker.scale.x, marker.scale.y, marker.scale.z])
int_marker.pose = obj.pose
int_marker.name = obj.key
marker.color.r = obj.shape.color[0]
marker.color.g = obj.shape.color[1]
marker.color.b = obj.shape.color[2]
marker.color.a = obj.shape.color[3]

# Main Motion
control = InteractiveMarkerControl()
control.always_visible = True
control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control.markers.append(marker)
int_marker.controls.append(control)

# Axis Movement in X
control = InteractiveMarkerControl()
control.name = "move_x"
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
self.normalizeQuaternion(control.orientation)
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)

# Axis Movement in Y
control = InteractiveMarkerControl()
control.name = "move_y"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
self.normalizeQuaternion(control.orientation)
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)

# Axis Movement in Z
control = InteractiveMarkerControl()
control.name = "move_z"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
self.normalizeQuaternion(control.orientation)
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)

# Axis Rotation in Z
control = InteractiveMarkerControl()
control.name = "rotate_z"
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
self.normalizeQuaternion(control.orientation)
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)

obj.marker_created = True
self.interactive_marker_server.insert(marker=int_marker, feedback_cb=self.int_marker_cb)
self.interactive_marker_server.applyChanges()


def marker_publisher(self):
"""
DEPRECATED -> see interactive_marker_creation method
Gets any existing dynamic objects (if added) and publishes them for debugging/display in RVIZ
Currently handles only:
- Sphere
Expand All @@ -3125,21 +3280,21 @@ def marker_publisher(self):
marker.header.stamp = rospy.Time.now()

if obj.shape.stype == 'sphere':
marker.type = 2
marker.type = Marker.SPHERE

# Expects diameter (m)
marker.scale.x = obj.shape.radius * 2
marker.scale.y = obj.shape.radius * 2
marker.scale.z = obj.shape.radius * 2
elif obj.shape.stype == 'cylinder':
marker.type = 3
marker.type = Marker.CYLINDER

# Expects diameter (m)
marker.scale.x = obj.shape.radius * 2
marker.scale.y = obj.shape.radius * 2
marker.scale.z = obj.shape.length
elif obj.shape.stype == 'cuboid':
marker.type = 1
marker.type = Marker.CUBE

# Expects diameter (m)
marker.scale.x = obj.shape.scale[0]
Expand Down Expand Up @@ -3263,12 +3418,15 @@ def step(self, dt: float = 0.01) -> None: # pylint: disable=unused-argument
self.last_tick = current_time

self.state_publisher.publish(self.state)
# Publishes any dynamically added shapes/collision objects to RVIZ for debugging
# TODO: add a way to enable/disable this
self.marker_publisher()

self.event.set()

def _load_shape_config(self):
"""
TODO: Attempts to load in any configured collision shapes
"""
pass

def __load_config(self):
"""[summary]
"""
Expand Down
9 changes: 8 additions & 1 deletion armer/utils.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,13 @@ def ikine(robot, target, q0, end):
return type('obj', (object,), {'q' : np.array(result[0])})

def trapezoidal(robot: rtb.Robot, qf: np.ndarray, max_speed=None, frequency=500, move_time_sec: float=5):
"""
Main Trajectory Creation Method
- Uses the in-built roboticstoolbox mtraj method
- Creates a trapezoidal trajectory (closely resembles minimum jerk)
- Uses provided frequency to construct trajectory time steps
- Uses move time to scale joint velocities.
"""
rospy.loginfo(f"[TRAJECTORY CONFIG] -> Trapezoidal Method | movement time: {move_time_sec} (sec)")

# ------------ NOTE: determine a joint trajectory (curved) based on time request
Expand All @@ -54,7 +61,7 @@ def mjtg(robot: rtb.robot, qf: np.ndarray, max_speed: float=0.2, max_rot: float=
# This is the average cartesian speed we want the robot to move at
# NOTE: divided by approx. 2 to make the max speed the approx. peak of the speed achieved
# TODO: investigate a better approximation strategy here
rospy.loginfo("Attempting to produce a mjtg tragectory")
rospy.logwarn(f"DEPRECATED. Please use trapezoidal method")
ave_cart_speed = max_speed / 1.92

#---------------- Calculate Linear move time estimate (3 point sampling)
Expand Down

0 comments on commit ac36e18

Please sign in to comment.