Exploration Node

nodes/edge_detection.py

@@ -28,7 +28,7 @@ def __init__(self, robot, threshold):
         self.y_min = sys.maxsize
         self.y_max = -sys.maxsize - 1
         self.threshold = threshold
-        self.find_corners()
+        #self.find_corners()
 
     def find_corners(self):
         """
@@ -38,7 +38,7 @@ def find_corners(self):
         self.robot.set_lift_height(1).wait_for_completed()
 
         # The order in which cozmo will turn and find edges
-        for angle in [180, 90, 180, 180]:
+        for angle in [180, 91, 180, 180]:
             while not self.robot.is_cliff_detected:
                 self.robot.drive_wheels(100, 100, duration=0.25)
             x_curr = self.robot.pose.position.x
@@ -50,13 +50,17 @@ def find_corners(self):
             self.y_max = max(y_curr, self.y_max)
             self.robot.drive_wheels(-100, -100, duration=1.5)
             self.robot.turn_in_place(degrees(angle)).wait_for_completed()
+        self.x_max = self.x_max - self.x_min
+        self.x_min = 0
+        self.y_max = self.y_max - self.y_min
+        self.y_min = 0
 
     def within_bounds(self, x_coord, y_coord):
         """
         Given x, y coordinates (in mm), returns if the coordinates are within bounds
         """
-        return self.x_min + self.threshold <= x_coord <= self.x_max - self.threshold and \
-               self.y_min + self.threshold <= y_coord <= self.y_max - self.threshold
+        return self.x_min + self.threshold <= y_coord <= self.x_max - self.threshold and \
+               self.y_min + self.threshold <= x_coord <= self.y_max - self.threshold
 
     def publish_plane(self, pub, height, boundary=False, color=(0, 0.5, 0.5)):
         """
@@ -72,13 +76,14 @@ def publish_plane(self, pub, height, boundary=False, color=(0, 0.5, 0.5)):
         color : tuple
             (r, g, b) values for the color of the surface
         All position and scale units are in mm
+        X and Y are switched because of the way cozmo moves in rviz
         """
         plane_marker = Marker()
         plane_marker.header.frame_id = "base_link"
         plane_marker.type = Marker.CUBE
 
-        plane_marker.pose.position.x = 0
-        plane_marker.pose.position.y = 0
+        plane_marker.pose.position.x = self.y_max / 2000
+        plane_marker.pose.position.y = self.x_max / 2000
         plane_marker.pose.position.z = height / 1000
 
         plane_marker.pose.orientation.x = 0
@@ -88,8 +93,8 @@ def publish_plane(self, pub, height, boundary=False, color=(0, 0.5, 0.5)):
 
         x_scale = (self.x_max - self.x_min) / 1000
         y_scale = (self.y_max - self.y_min) / 1000
-        plane_marker.scale.x = x_scale if not boundary else x_scale - 2 * self.threshold / 1000
-        plane_marker.scale.y = y_scale if not boundary else y_scale - 2 * self.threshold / 1000
+        plane_marker.scale.y = x_scale if not boundary else x_scale - 2 * self.threshold / 1000
+        plane_marker.scale.x = y_scale if not boundary else y_scale - 2 * self.threshold / 1000
         plane_marker.scale.z = 1 / 1000
 
         plane_marker.color.r = color[0]

nodes/explore.py

@@ -0,0 +1,174 @@
+#!/usr/bin/env python3
+import sys
+sys.path.append('../objects')
+from edge_detection import CornerDetector
+from custom_object import RectangularCuboid
+import rospy
+
+import cozmo
+from cozmo.util import angle_z_to_quaternion, pose_z_angle, radians, degrees
+from cozmo.util import distance_mm, speed_mmps
+import threading
+
+from visualization_msgs.msg import Marker
+from roscpp_initializer import roscpp_initializer
+from cozmopy import Cozmo, ObjectOrientedActionSpace
+from aikidopy import InteractiveMarkerViewer
+
+import random
+
+
+def look_for_object(robot: cozmo.robot, cube, timeout=2):
+    """
+    Tells Cozmo to look around for two LightCubes and constructs a CustomObject
+    representing them
+    """
+    done = False
+    while not done:
+        try:
+            look_around = robot.start_behavior(
+                cozmo.behavior.BehaviorTypes.LookAroundInPlace)
+            cubes = robot.world.wait_until_observe_num_objects(2, cozmo.objects.LightCube, timeout=timeout)
+            if cubes:
+                done = True
+            else:
+                look_around.stop()
+                val = input("Cozmo can't find any cubes, please reset cozmo and cubes and then press Enter to continue")
+                if val == '-1':
+                    sys.exit(0)
+        except asyncio.TimeoutError:
+            continue
+        finally:
+            look_around.stop()
+    # cube = RectangularCuboid(2, (0, 0, 0), 45)
+    # cube.update_object(cubes)
+    return cubes
+
+def publish_cozmo(pub, robot, cozmo):
+    """
+    Thread to publish cozmo's location asynchronously
+    """
+    while not rospy.is_shutdown():
+        q = robot.pose.rotation
+        quat = [q.q0, q.q1, q.q2, q.q3]
+        cozmo.setState(
+            robot.pose.position.x / 1000.0,
+            robot.pose.position.y / 1000.0,
+            quat)
+        rospy.sleep(0.001)
+
+def cozmo_run(robot: cozmo.robot):
+    topicName = "cozmo_model"
+    baseFrameName = "base_link"
+    custom_obj = RectangularCuboid(2, (0, 0, 0), 45)
+
+    if not rospy.is_shutdown():
+        cozmo = Cozmo("/home/bubbletea/cozmo_ws/src/libcozmo/src/cozmo_description/meshes")
+        skeleton = cozmo.getCozmoSkeleton();
+        fake_cozmo = Cozmo("/home/bubbletea/cozmo_ws/src/libcozmo/src/cozmo_description/meshes")
+        fake_skeleton = cozmo.getCozmoSkeleton();
+
+        viewer = InteractiveMarkerViewer(topicName, baseFrameName)
+        cozmo_marker = viewer.addSkeleton(skeleton)
+        fake_cozmo_marker = viewer.addSkeleton(fake_skeleton)
+        viewer.setAutoUpdate(True);
+
+    cozmo_publisher = rospy.Publisher('cozmo_marker', Marker, queue_size=10)
+    object_publisher = rospy.Publisher('object_marker', Marker, queue_size=10)
+    plane_publisher = rospy.Publisher('plane_marker', Marker, queue_size=10)
+    boundary_publisher = rospy.Publisher('boundary_marker', Marker, queue_size=10)
+    cozmo_start_publisher = rospy.Publisher('cozmo_start_marker', Marker, queue_size=10)
+
+    t = threading.Thread(
+        target=publish_cozmo,
+        args=(cozmo_publisher, robot, cozmo))
+    t.start()
+
+    #look_for_duration = float(input('How long to look for cubes? '))
+    #threshold = float(input('Boundary threshold? '))
+    #detector = CornerDetector(robot, threshold)
+    detector = CornerDetector(robot, 60)
+    # Manually set dimensions
+    detector.x_min, detector.x_max, detector.y_min, detector.y_max = 0, 609, 0, 1524
+    input('Reset Cozmo to the bottom left of the surface for alignment')
+
+    robot.turn_in_place(degrees(30)).wait_for_completed()
+    robot.drive_wheels(100, 100, duration=2)
+
+    while not rospy.is_shutdown():
+        # detector.publish_plane(plane_publisher, 0, color=(1, 0.5, 0))
+        # detector.publish_plane(boundary_publisher, 2, True)
+
+        custom_obj.update_object(look_for_object(robot, custom_obj, 5))
+        custom_obj.publish_cube(object_publisher, color=(1, 0.2, 0, 1))
+        rospy.sleep(0.001)
+
+        actionspace = ObjectOrientedActionSpace([10, 100], [44, 89], [30, 30], [50, 95], 3)
+
+        state = cozmo.createState(custom_obj.pose[0], custom_obj.pose[1], custom_obj.pose[2])
+        action_id = random.choice([i for i in range(actionspace.size())])
+        action = actionspace.get_generic_to_object_oriented_action(action_id, state)
+
+        # publishing where we want cozmo to go
+        publish_cozmo_start(cozmo_start_publisher, [action.start_pose()[0], action.start_pose()[1], angle_z_to_quaternion(radians(action.start_pose()[2]))])
+
+        # # Part 1 of oo action: getting to object
+        robot.go_to_pose(pose_z_angle(action.start_pose()[0],
+            action.start_pose()[1],
+            0,
+            radians(action.start_pose()[2]))).wait_for_completed()
+
+        # # Part 2 of oo action: pushing object
+        robot.drive_wheels(action.speed(), action.speed(), duration=3)
+
+        # x_coord = robot.pose.position.x
+        # y_coord = robot.pose.position.y
+
+        # print('Cozmo position: ', (x_coord, y_coord), 'table coords: ', (detector.y_max, detector.x_max, '\n'))
+        # # check if cozmo is within bounds:
+        # if not detector.within_bounds(x_coord, y_coord):
+        #     input('Cozmo is out of bounds, please reset and press Enter to continue')
+        # else:
+        #     print('within bounds')
+        #     # update model 
+
+        # # Optional command that tells cozmo to move back a bit to detect cubes better
+        # robot.drive_straight(
+        #     distance_mm(-100),
+        #     speed_mmps(100)).wait_for_completed()
+
+def publish_cozmo_start(publisher, cozmo_pose, color=(0, 0, 0, 1)):
+    cozmo_marker = Marker()
+    cozmo_marker.header.frame_id = "base_link"
+    cozmo_marker.type = Marker.CUBE
+
+    cozmo_marker.pose.position.x = (cozmo_pose[0]) / 1000
+    cozmo_marker.pose.position.y = (cozmo_pose[1]) / 1000
+    cozmo_marker.pose.position.z = 0
+
+    cozmo_marker.pose.orientation.x = cozmo_pose[2][1]
+    cozmo_marker.pose.orientation.y = cozmo_pose[2][2]
+    cozmo_marker.pose.orientation.z = cozmo_pose[2][3]
+    cozmo_marker.pose.orientation.w = cozmo_pose[2][0]
+
+    cozmo_marker.scale.y = 45 / 1000
+    cozmo_marker.scale.x = 89 / 1000
+    cozmo_marker.scale.z = 45 / 1000
+
+    cozmo_marker.color.r = color[0]
+    cozmo_marker.color.g = color[1]
+    cozmo_marker.color.b = color[2]
+    cozmo_marker.color.a = 1
+
+    publisher.publish(cozmo_marker)
+
+
+if __name__ == '__main__':
+    roscpp_initializer.roscpp_init("custom_object", [])
+    rospy.init_node("custom_object")
+    rate = rospy.Rate(10)
+
+    try:
+        cozmo.run_program(cozmo_run)
+    except rospy.ROSInterruptException:
+        pass

objects/custom_object.py

@@ -22,6 +22,20 @@ def __init__(self, num_cubes, pose, side_length):
         self.width = side_length
         self.height = side_length
 
+        self.cube_marker = Marker()
+        self.cube_marker.header.frame_id = "base_link"
+        self.cube_marker.type = Marker.CUBE
+
+        color=(0, 0.5, 0.5, 1)
+
+        self.cube_marker.scale.x = self.width / 1000
+        self.cube_marker.scale.y = self.length / 1000
+        self.cube_marker.scale.z = self.width / 1000
+        self.cube_marker.color.r = color[0]
+        self.cube_marker.color.g = color[1]
+        self.cube_marker.color.b = color[2]
+        self.cube_marker.color.a = color[3]
+
     def update_object(self, cubes):
         """
         Updates the object's pose given the cubes
@@ -53,29 +67,17 @@ def publish_cube(self, publisher, color=(0, 0.5, 0.5, 1)):
         color : tuple
             (r, g, b, a) to represent the color of the cube
         """
-        cube_marker = Marker()
-        cube_marker.header.frame_id = "base_link"
-        cube_marker.type = Marker.CUBE
-
-        cube_marker.pose.position.x = self.pose[0] / 1000
-        cube_marker.pose.position.y = self.pose[1] / 1000
-        cube_marker.pose.position.z = self.height / 1000
+        self.cube_marker.pose.position.x = self.pose[0] / 1000
+        self.cube_marker.pose.position.y = self.pose[1] / 1000
+        self.cube_marker.pose.position.z = self.height / 1000
 
         cube_orientation = angle_z_to_quaternion(radians(self.pose[2]))
-        cube_marker.pose.orientation.x = cube_orientation[1]
-        cube_marker.pose.orientation.y = cube_orientation[2]
-        cube_marker.pose.orientation.z = cube_orientation[3]
-        cube_marker.pose.orientation.w = cube_orientation[0]
-
-        cube_marker.scale.x = self.width / 1000
-        cube_marker.scale.y = self.length / 1000
-        cube_marker.scale.z = self.width / 1000
-        cube_marker.color.r = color[0]
-        cube_marker.color.g = color[1]
-        cube_marker.color.b = color[2]
-        cube_marker.color.a = color[3]
+        self.cube_marker.pose.orientation.x = cube_orientation[1]
+        self.cube_marker.pose.orientation.y = cube_orientation[2]
+        self.cube_marker.pose.orientation.z = cube_orientation[3]
+        self.cube_marker.pose.orientation.w = cube_orientation[0]
 
-        publisher.publish(cube_marker)
+        publisher.publish(self.cube_marker)
 
     def __str__(self):
         return "Pose: %s, Length: %s, Width: %s" % \

src/cozmo_description/cozmo.cpp

@@ -73,6 +73,7 @@ BodyNodePtr Cozmo::makeRootBody(const SkeletonPtr& cozmo,
     Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
 
     R = Eigen::AngleAxisd(-M_PI/2, Eigen::Vector3d::UnitX());
+    tf.translation() << 0.035, 0, 0.058;
     tf.linear() = R;
 
     bn->getParentJoint()->setTransformFromChildBodyNode(tf);
@@ -85,7 +86,7 @@ BodyNodePtr Cozmo::addBody(const SkeletonPtr& cozmo,
 			   BodyNodePtr parent,
 			   const std::string& mesh_name,
 			   const std::string& mesh_dir,
-	                   Eigen::Vector3d transformFromParent,
+	           Eigen::Vector3d transformFromParent,
 			   Eigen::Vector3d transformFromChild)
 {
     RevoluteJoint::Properties properties;

src/cozmopy/cozmopy.cpp

@@ -1,7 +1,9 @@
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
 #include <pybind11/chrono.h>
+#include <pybind11/eigen.h>
 #include <cozmo_description/cozmo.hpp>
+#include "actionspace/ObjectOrientedActionSpace.hpp"
 #include <chrono>
 #include <Eigen/Geometry>
 
@@ -45,6 +47,56 @@ PYBIND11_MODULE(cozmopy, m)
 			libcozmo::Waypoint w = {.x = x, .y = y, .th = th, .t = t};
 			return w;
 		}));
+
+	py::class_<actionspace::ObjectOrientedActionSpace>(m, "ObjectOrientedActionSpace")
+		.def(py::init<const std::vector<double>&,
+					  const std::vector<double>&,
+					  const Eigen::Vector2d&,
+					  const Eigen::Vector2d&,
+					  const int&>())
+		.def("action_similarity", [](
+			const actionspace::ObjectOrientedActionSpace& actionspace,
+			const int& action_id1, 
+			const int& action_id2) {
+			double similarity = -1;
+			bool successful = 
+				actionspace.action_similarity(action_id1, action_id2, &similarity);
+			return similarity, successful;
+		})
+		.def("get_action", 
+			 &actionspace::ObjectOrientedActionSpace::get_action, 
+			 py::arg("action_id"))
+		.def("is_valid_action_id",
+			 &actionspace::ObjectOrientedActionSpace::is_valid_action_id,
+			 py::arg("action_id"))
+		.def("get_generic_to_object_oriented_action",[](
+			const actionspace::ObjectOrientedActionSpace& actionspace,
+			const int& action_id,
+        	const aikido::statespace::StateSpace::State& _state){
+			actionspace::ObjectOrientedActionSpace::ObjectOrientedAction action(
+				0.0, Eigen::Vector3d(0, 0, 0));
+			bool successful = actionspace.get_generic_to_object_oriented_action(
+				action_id, _state, &action);
+			return action;
+		})
+		.def("publish_action",
+			 &actionspace::ObjectOrientedActionSpace::publish_action,
+			 py::arg("action_id"),
+			 py::arg("publisher"),
+			 py::arg("_state"))
+		.def("size", &actionspace::ObjectOrientedActionSpace::size);
+
+		py::class_<actionspace::ObjectOrientedActionSpace::GenericAction>(m, "GenericAction")
+			.def(py::init<const double&, const double&, const double&, const double&>())
+			.def("speed", &actionspace::ObjectOrientedActionSpace::GenericAction::speed)
+			.def("aspect_ratio", &actionspace::ObjectOrientedActionSpace::GenericAction::aspect_ratio)
+			.def("edge_offset", &actionspace::ObjectOrientedActionSpace::GenericAction::edge_offset)
+			.def("heading_offset", &actionspace::ObjectOrientedActionSpace::GenericAction::heading_offset);
+
+		py::class_<actionspace::ObjectOrientedActionSpace::ObjectOrientedAction>(m, "ObjectOrientedAction")
+			.def(py::init<const double&, const Eigen::Vector3d&>())
+			.def("speed", &actionspace::ObjectOrientedActionSpace::ObjectOrientedAction::speed)
+			.def("start_pose", &actionspace::ObjectOrientedActionSpace::ObjectOrientedAction::start_pose);
 }
 
 }  // namespace python