diff --git a/src/active_bo_msgs/CMakeLists.txt b/src/active_bo_msgs/CMakeLists.txt
index 3b65401..a41871c 100644
--- a/src/active_bo_msgs/CMakeLists.txt
+++ b/src/active_bo_msgs/CMakeLists.txt
@@ -18,6 +18,7 @@ endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
+find_package(geometry_msgs REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"srv/WeightToPolicy.srv"
@@ -33,7 +34,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/ActiveRLEvalResponse.msg"
"msg/ImageFeedback.msg"
"msg/ActiveBOState.msg"
-
+ "msg/DMP.msg"
+ DEPENDENCIES geometry_msgs
)
if(BUILD_TESTING)
diff --git a/src/active_bo_msgs/msg/DMP.msg b/src/active_bo_msgs/msg/DMP.msg
new file mode 100644
index 0000000..f8f780f
--- /dev/null
+++ b/src/active_bo_msgs/msg/DMP.msg
@@ -0,0 +1,13 @@
+# Start- and endpoint, time to perform the trajectory
+geometry_msgs/Pose start_point
+geometry_msgs/Pose end_point
+float64 time
+
+# weights for the dimensions of the Pose (3 Position, 4 Orientation)
+float64[] p_x
+float64[] p_y
+float64[] p_z
+float64[] o_x
+float64[] o_y
+float64[] o_z
+float64[] o_w
\ No newline at end of file
diff --git a/src/active_bo_msgs/package.xml b/src/active_bo_msgs/package.xml
index fd8e609..a1a7e4b 100644
--- a/src/active_bo_msgs/package.xml
+++ b/src/active_bo_msgs/package.xml
@@ -7,6 +7,8 @@
cpsfeith
TODO: License declaration
+ geometry_msgs
+
ament_cmake
rosidl_default_generators
diff --git a/src/active_bo_ros/active_bo_ros/AcquisitionFunctions/ConfidenceBound.py b/src/active_bo_ros/active_bo_ros/AcquisitionFunctions/ConfidenceBound.py
index dad3927..46f05d2 100644
--- a/src/active_bo_ros/active_bo_ros/AcquisitionFunctions/ConfidenceBound.py
+++ b/src/active_bo_ros/active_bo_ros/AcquisitionFunctions/ConfidenceBound.py
@@ -1,3 +1,4 @@
+
import numpy as np
diff --git a/src/active_bo_ros/active_bo_ros/PolicyModel/GaussianModelMultiDim.py b/src/active_bo_ros/active_bo_ros/PolicyModel/GaussianModelMultiDim.py
index befbb22..4b97d1e 100644
--- a/src/active_bo_ros/active_bo_ros/PolicyModel/GaussianModelMultiDim.py
+++ b/src/active_bo_ros/active_bo_ros/PolicyModel/GaussianModelMultiDim.py
@@ -1,3 +1,4 @@
+
import numpy as np