Skip to content

Commit

Permalink
debug expressions are stored raw
Browse files Browse the repository at this point in the history
  • Loading branch information
@ichumuh
ichumuh committed Aug 30, 2024
1 parent c2a9eed commit 2163bf6
Show file tree
Hide file tree
Showing 5 changed files with 164 additions and 154 deletions.
25 changes: 13 additions & 12 deletions src/giskardpy/debug_expression_manager.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,23 +16,19 @@

class DebugExpressionManager:
debug_expressions: Dict[PrefixName, cas.Expression]
compiled_debug_expressions: Dict[str, cas.CompiledFunction]
evaluated_debug_expressions: Dict[str, np.ndarray]
_debug_trajectory: Trajectory
compiled_debug_expressions: Dict[PrefixName, cas.CompiledFunction]
evaluated_debug_expressions: Dict[PrefixName, np.ndarray]
_raw_debug_trajectory: List[Dict[PrefixName, np.ndarray]]

def __init__(self):
self.debug_expressions = {}
self._debug_trajectory = Trajectory()
self._raw_debug_trajectory = []

def add_debug_expression(self, name: str, expression: cas.Expression, color: Optional[ColorRGBA] = None):
if isinstance(expression, cas.Symbol_):
expression.color = color
self.debug_expressions[PrefixName(name, prefix='')] = expression

@property
def debug_trajectory(self):
return self._debug_trajectory

def compile_debug_expressions(self):
for name, expr in self.debug_expressions.items():
if isinstance(expr, (int, float)):
Expand Down Expand Up @@ -60,12 +56,16 @@ def eval_debug_expressions(self, log_traj: bool = True): # renamed

def log_debug_expressions(self):
if len(self.evaluated_debug_expressions) > 0:
control_cycle_counter = god_map.control_cycle_counter - 1
self._raw_debug_trajectory.append(self.evaluated_debug_expressions)

def raw_traj_to_traj(self) -> Trajectory:
debug_trajectory = Trajectory()
for control_cycle_counter, evaluated_debug_expressions in enumerate(self._raw_debug_trajectory):
last_mjs = None
if control_cycle_counter >= 1:
last_mjs = self._debug_trajectory.get_exact(control_cycle_counter - 1)
last_mjs = debug_trajectory.get_exact(control_cycle_counter - 1)
js = JointStates()
for name, value in self.evaluated_debug_expressions.items():
for name, value in evaluated_debug_expressions.items():
if len(value) > 1:
if len(value.shape) == 2:
for x in range(value.shape[0]):
Expand All @@ -78,7 +78,8 @@ def log_debug_expressions(self):
self.evaluated_expr_to_js(tmp_name, last_mjs, js, value[x])
else:
self.evaluated_expr_to_js(name, last_mjs, js, value)
self._debug_trajectory.set(control_cycle_counter, js)
debug_trajectory.set(control_cycle_counter, js)
return debug_trajectory

def evaluated_expr_to_js(self, name, last_js, next_js: JointStates, value):
if last_js is not None:
Expand Down
143 changes: 141 additions & 2 deletions src/giskardpy/model/ros_msg_visualization.py
View file
Original file line number Diff line number Diff line change
@@ -1,13 +1,17 @@
from copy import deepcopy
from enum import Enum
from typing import Optional, List
from typing import Optional, List, Dict, Union

from tf.transformations import rotation_matrix, quaternion_from_matrix

import giskardpy.casadi_wrapper as cas
import numpy as np
import rospy
from geometry_msgs.msg import Vector3, Point
from geometry_msgs.msg import Vector3, Point, Quaternion
from std_msgs.msg import ColorRGBA
from visualization_msgs.msg import MarkerArray, Marker

from giskardpy.data_types import JointStates, PrefixName
from giskardpy.god_map import god_map
from giskardpy.model.collision_world_syncer import Collisions, Collision
from giskardpy.model.trajectory import Trajectory
Expand All @@ -27,6 +31,16 @@ class ROSMsgVisualization:
red = ColorRGBA(1.0, 0.0, 0.0, 1.0)
yellow = ColorRGBA(1.0, 1.0, 0.0, 1.0)
green = ColorRGBA(0.0, 1.0, 0.0, 1.0)
colors = [
red, # red
ColorRGBA(r=0.0, g=0.0, b=1.0, a=1.0), # blue
yellow, # yellow
ColorRGBA(r=1.0, g=0.0, b=1.0, a=1.0), # violet
ColorRGBA(r=0.0, g=1.0, b=1.0, a=1.0), # cyan
green, # green
ColorRGBA(r=1.0, g=1.0, b=1.0, a=1.0), # white
ColorRGBA(r=0.0, g=0.0, b=0.0, a=1.0), # black
]
mode: VisualizationMode
frame_locked: bool
world_version: int
Expand Down Expand Up @@ -188,3 +202,128 @@ def clear_marker(self, ns: str):
msg.markers.append(marker)
self.publisher.publish(msg)
self.marker_ids = {}

def debug_state_to_vectors_markers(self,
debug_expressions: Dict[PrefixName, Union[cas.TransMatrix,
cas.Point3,
cas.Vector3,
cas.Quaternion]],
debug_values: Dict[PrefixName, np.ndarray],
width: float = 0.05) -> List[Marker]:
ms = []
color_counter = 0
for (name, expr), (_, value) in zip(debug_expressions.items(), debug_values.items()):
if not hasattr(expr, 'reference_frame'):
continue
if expr.reference_frame is not None:
map_T_ref = god_map.world.compute_fk_np(god_map.world.root_link_name, expr.reference_frame)
else:
map_T_ref = np.eye(4)
if isinstance(expr, cas.TransMatrix):
ref_T_d = value
map_T_d = np.dot(map_T_ref, ref_T_d)
map_P_d = map_T_d[:4, 3:]
# x
d_V_x_offset = np.array([width, 0, 0, 0])
map_V_x_offset = np.dot(map_T_d, d_V_x_offset)
mx = Marker()
mx.action = mx.ADD
mx.header.frame_id = self.tf_root
mx.ns = f'debug{name}'
mx.id = 0
mx.type = mx.CYLINDER
mx.pose.position.x = map_P_d[0][0] + map_V_x_offset[0]
mx.pose.position.y = map_P_d[1][0] + map_V_x_offset[1]
mx.pose.position.z = map_P_d[2][0] + map_V_x_offset[2]
d_R_x = rotation_matrix(np.pi / 2, [0, 1, 0])
map_R_x = np.dot(map_T_d, d_R_x)
mx.pose.orientation = Quaternion(*quaternion_from_matrix(map_R_x))
mx.color = ColorRGBA(1, 0, 0, 1)
mx.scale.x = width / 4
mx.scale.y = width / 4
mx.scale.z = width * 2
ms.append(mx)
# y
d_V_y_offset = np.array([0, width, 0, 0])
map_V_y_offset = np.dot(map_T_d, d_V_y_offset)
my = Marker()
my.action = my.ADD
my.header.frame_id = self.tf_root
my.ns = f'debug{name}'
my.id = 1
my.type = my.CYLINDER
my.pose.position.x = map_P_d[0][0] + map_V_y_offset[0]
my.pose.position.y = map_P_d[1][0] + map_V_y_offset[1]
my.pose.position.z = map_P_d[2][0] + map_V_y_offset[2]
d_R_y = rotation_matrix(-np.pi / 2, [1, 0, 0])
map_R_y = np.dot(map_T_d, d_R_y)
my.pose.orientation = Quaternion(*quaternion_from_matrix(map_R_y))
my.color = ColorRGBA(0, 1, 0, 1)
my.scale.x = width / 4
my.scale.y = width / 4
my.scale.z = width * 2
ms.append(my)
# z
d_V_z_offset = np.array([0, 0, width, 0])
map_V_z_offset = np.dot(map_T_d, d_V_z_offset)
mz = Marker()
mz.action = mz.ADD
mz.header.frame_id = self.tf_root
mz.ns = f'debug{name}'
mz.id = 2
mz.type = mz.CYLINDER
mz.pose.position.x = map_P_d[0][0] + map_V_z_offset[0]
mz.pose.position.y = map_P_d[1][0] + map_V_z_offset[1]
mz.pose.position.z = map_P_d[2][0] + map_V_z_offset[2]
mz.pose.orientation = Quaternion(*quaternion_from_matrix(map_T_d))
mz.color = ColorRGBA(0, 0, 1, 1)
mz.scale.x = width / 4
mz.scale.y = width / 4
mz.scale.z = width * 2
ms.append(mz)
else:
m = Marker()
m.action = m.ADD
m.ns = f'debug/{name}'
m.id = 0
m.header.frame_id = self.tf_root
m.pose.orientation.w = 1
if isinstance(expr, cas.Vector3):
ref_V_d = value
if expr.vis_frame is not None:
map_T_vis = god_map.world.compute_fk_np(god_map.world.root_link_name, expr.vis_frame)
else:
map_T_vis = np.eye(4)
map_V_d = np.dot(map_T_ref, ref_V_d)
map_P_vis = map_T_vis[:4, 3:].T[0]
map_P_p1 = map_P_vis
map_P_p2 = map_P_vis + map_V_d * 0.5
m.points.append(Point(map_P_p1[0], map_P_p1[1], map_P_p1[2]))
m.points.append(Point(map_P_p2[0], map_P_p2[1], map_P_p2[2]))
m.type = m.ARROW
if expr.color is None:
m.color = self.colors[color_counter]
else:
m.color = expr.color
m.scale.x = width / 2
m.scale.y = width
m.scale.z = 0
color_counter += 1
elif isinstance(expr, cas.Point3):
ref_P_d = value
map_P_d = np.dot(map_T_ref, ref_P_d)
m.pose.position.x = map_P_d[0]
m.pose.position.y = map_P_d[1]
m.pose.position.z = map_P_d[2]
m.pose.orientation.w = 1
m.type = m.SPHERE
if expr.color is None:
m.color = self.colors[color_counter]
else:
m.color = expr.color
m.scale.x = width
m.scale.y = width
m.scale.z = width
color_counter += 1
ms.append(m)
return ms
146 changes: 8 additions & 138 deletions src/giskardpy/tree/behaviors/debug_marker_publisher.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,15 +18,7 @@


class DebugMarkerPublisher(GiskardBehavior):
colors = [ColorRGBA(r=1, g=0, b=0, a=1), # red
ColorRGBA(r=0, g=0, b=1, a=1), # blue
ColorRGBA(r=1, g=1, b=0, a=1), # yellow
ColorRGBA(r=1, g=0, b=1, a=1), # violet
ColorRGBA(r=0, g=1, b=1, a=1), # cyan
ColorRGBA(r=0, g=1, b=0, a=1), # green
ColorRGBA(r=1, g=1, b=1, a=1), # white
ColorRGBA(r=0, g=0, b=0, a=1), # black
]


@profile
def __init__(self, name: str = 'debug marker', tf_topic: str = '/tf', map_frame: Optional[str] = None):
Expand All @@ -43,131 +35,6 @@ def setup(self, timeout):
self.clear_markers()
return super().setup(timeout)

def publish_debug_markers(self):
ms = MarkerArray()
ms.markers.extend(self.to_vectors_markers())
self.marker_pub.publish(ms)

def to_vectors_markers(self, width: float = 0.05) -> List[Marker]:
ms = []
color_counter = 0
for name, value in self.debugs_evaluated.items():
expr = self.debugs[name]
if not hasattr(expr, 'reference_frame'):
continue
if expr.reference_frame is not None:
map_T_ref = god_map.world.compute_fk_np(god_map.world.root_link_name, expr.reference_frame)
else:
map_T_ref = np.eye(4)
if isinstance(expr, w.TransMatrix):
ref_T_d = value
map_T_d = np.dot(map_T_ref, ref_T_d)
map_P_d = map_T_d[:4, 3:]
# x
d_V_x_offset = np.array([width, 0, 0, 0])
map_V_x_offset = np.dot(map_T_d, d_V_x_offset)
mx = Marker()
mx.action = mx.ADD
mx.header.frame_id = self.map_frame
mx.ns = f'debug{name}'
mx.id = 0
mx.type = mx.CYLINDER
mx.pose.position.x = map_P_d[0][0] + map_V_x_offset[0]
mx.pose.position.y = map_P_d[1][0] + map_V_x_offset[1]
mx.pose.position.z = map_P_d[2][0] + map_V_x_offset[2]
d_R_x = rotation_matrix(np.pi / 2, [0, 1, 0])
map_R_x = np.dot(map_T_d, d_R_x)
mx.pose.orientation = Quaternion(*quaternion_from_matrix(map_R_x))
mx.color = ColorRGBA(1, 0, 0, 1)
mx.scale.x = width / 4
mx.scale.y = width / 4
mx.scale.z = width * 2
ms.append(mx)
# y
d_V_y_offset = np.array([0, width, 0, 0])
map_V_y_offset = np.dot(map_T_d, d_V_y_offset)
my = Marker()
my.action = my.ADD
my.header.frame_id = self.map_frame
my.ns = f'debug{name}'
my.id = 1
my.type = my.CYLINDER
my.pose.position.x = map_P_d[0][0] + map_V_y_offset[0]
my.pose.position.y = map_P_d[1][0] + map_V_y_offset[1]
my.pose.position.z = map_P_d[2][0] + map_V_y_offset[2]
d_R_y = rotation_matrix(-np.pi / 2, [1, 0, 0])
map_R_y = np.dot(map_T_d, d_R_y)
my.pose.orientation = Quaternion(*quaternion_from_matrix(map_R_y))
my.color = ColorRGBA(0, 1, 0, 1)
my.scale.x = width / 4
my.scale.y = width / 4
my.scale.z = width * 2
ms.append(my)
# z
d_V_z_offset = np.array([0, 0, width, 0])
map_V_z_offset = np.dot(map_T_d, d_V_z_offset)
mz = Marker()
mz.action = mz.ADD
mz.header.frame_id = self.map_frame
mz.ns = f'debug{name}'
mz.id = 2
mz.type = mz.CYLINDER
mz.pose.position.x = map_P_d[0][0] + map_V_z_offset[0]
mz.pose.position.y = map_P_d[1][0] + map_V_z_offset[1]
mz.pose.position.z = map_P_d[2][0] + map_V_z_offset[2]
mz.pose.orientation = Quaternion(*quaternion_from_matrix(map_T_d))
mz.color = ColorRGBA(0, 0, 1, 1)
mz.scale.x = width / 4
mz.scale.y = width / 4
mz.scale.z = width * 2
ms.append(mz)
else:
m = Marker()
m.action = m.ADD
m.ns = f'debug/{name}'
m.id = 0
m.header.frame_id = self.map_frame
m.pose.orientation.w = 1
if isinstance(expr, w.Vector3):
ref_V_d = value
if expr.vis_frame is not None:
map_T_vis = god_map.world.compute_fk_np(god_map.world.root_link_name, expr.vis_frame)
else:
map_T_vis = np.eye(4)
map_V_d = np.dot(map_T_ref, ref_V_d)
map_P_vis = map_T_vis[:4, 3:].T[0]
map_P_p1 = map_P_vis
map_P_p2 = map_P_vis + map_V_d * 0.5
m.points.append(Point(map_P_p1[0], map_P_p1[1], map_P_p1[2]))
m.points.append(Point(map_P_p2[0], map_P_p2[1], map_P_p2[2]))
m.type = m.ARROW
if expr.color is None:
m.color = self.colors[color_counter]
else:
m.color = expr.color
m.scale.x = width / 2
m.scale.y = width
m.scale.z = 0
color_counter += 1
elif isinstance(expr, w.Point3):
ref_P_d = value
map_P_d = np.dot(map_T_ref, ref_P_d)
m.pose.position.x = map_P_d[0]
m.pose.position.y = map_P_d[1]
m.pose.position.z = map_P_d[2]
m.pose.orientation.w = 1
m.type = m.SPHERE
if expr.color is None:
m.color = self.colors[color_counter]
else:
m.color = expr.color
m.scale.x = width
m.scale.y = width
m.scale.z = width
color_counter += 1
ms.append(m)
return ms

def clear_markers(self):
msg = MarkerArray()
marker = Marker()
Expand All @@ -178,8 +45,11 @@ def clear_markers(self):
@record_time
@profile
def update(self):
self.debugs = god_map.debug_expression_manager.debug_expressions
if len(self.debugs) > 0:
self.debugs_evaluated = god_map.debug_expression_manager.evaluated_debug_expressions
self.publish_debug_markers()
debug_exprs = god_map.debug_expression_manager.debug_expressions
if len(debug_exprs) > 0:
debug_state = god_map.debug_expression_manager.evaluated_debug_expressions
ms = MarkerArray()
markers = god_map.ros_visualizer.debug_state_to_vectors_markers(debug_exprs, debug_state)
ms.markers.extend(markers)
self.marker_pub.publish(ms)
return Status.SUCCESS
2 changes: 1 addition & 1 deletion src/giskardpy/tree/behaviors/plot_debug_expressions.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ def split_traj(self, traj) -> Trajectory:
return new_traj

def plot(self):
trajectory = god_map.debug_expression_manager.debug_trajectory
trajectory = god_map.debug_expression_manager.raw_traj_to_traj()
if trajectory and len(trajectory.items()) > 0:
sample_period = god_map.qp_controller_config.sample_period
traj = self.split_traj(trajectory)
Expand Down
Loading

0 comments on commit 2163bf6

Please sign in to comment.