diff --git a/nodes/edge_detection.py b/nodes/edge_detection.py index e0ca4539..cc27a9b2 100755 --- a/nodes/edge_detection.py +++ b/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] diff --git a/nodes/explore.py b/nodes/explore.py new file mode 100755 index 00000000..c79b032e --- /dev/null +++ b/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 diff --git a/objects/custom_object.py b/objects/custom_object.py index 1a72fad7..ea20ece6 100755 --- a/objects/custom_object.py +++ b/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" % \ diff --git a/src/cozmo_description/cozmo.cpp b/src/cozmo_description/cozmo.cpp index 52bb279f..d4c91a28 100644 --- a/src/cozmo_description/cozmo.cpp +++ b/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; diff --git a/src/cozmopy/cozmopy.cpp b/src/cozmopy/cozmopy.cpp index 552eb768..77c3c36a 100644 --- a/src/cozmopy/cozmopy.cpp +++ b/src/cozmopy/cozmopy.cpp @@ -1,7 +1,9 @@ #include #include #include +#include #include +#include "actionspace/ObjectOrientedActionSpace.hpp" #include #include @@ -45,6 +47,56 @@ PYBIND11_MODULE(cozmopy, m) libcozmo::Waypoint w = {.x = x, .y = y, .th = th, .t = t}; return w; })); + + py::class_(m, "ObjectOrientedActionSpace") + .def(py::init&, + const std::vector&, + 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_(m, "GenericAction") + .def(py::init()) + .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_(m, "ObjectOrientedAction") + .def(py::init()) + .def("speed", &actionspace::ObjectOrientedActionSpace::ObjectOrientedAction::speed) + .def("start_pose", &actionspace::ObjectOrientedActionSpace::ObjectOrientedAction::start_pose); } } // namespace python diff --git a/src/examples/exploration_strategies.py b/src/examples/exploration_strategies.py new file mode 100644 index 00000000..735631b0 --- /dev/null +++ b/src/examples/exploration_strategies.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python3 + +from cozmopy import ObjectOrientedActionSpace + +class ExplorationPolicies(object): + def __init__(actionspace): + self.actionspace = actionspace + self.random = Random(self.actionspace) + +class Random: + def __init__(actionspace): + self.actionspace = actionspace + + def action(self, state): + return random.choice([i for i in range(self.actionspace.size())]) + +class Novelty: + def __init__(actionspace): + self.actionspace = actionspace + # action_id -> num times explored + self.num_explored = {} + self.actions = [i for i in range(self.actionspace.size())] + + def get_explored_action(self): + if len(self.num_explored) >= self.actionspace.size(): + min_val = min(self.num_explored.values()) + min_explored = [k for k in self.num_explored.keys() if self.num_explored[k] == min_val] + max_explored = list(set(actions).difference(min_explored)) + else: + max_explored = self.num_explored.keys() + min_explored = list(set(actions).difference(max_explored)) + + return min_explored, max_explored + + def action(self, state): + min_explored, max_explored = self.get_explored_action() + if len(max_explored) == 0: + return self.get_random_action() + + most_novel_action = None + most_novel_action_score = 0 + for min_action in min_explored: + action_dists = [self.actionspace.action_similarity(min_action, max_action) for max_action in max_explored] + action_score = sum(action_dists)/len(action_dists) + if action_score > most_novel_action_score: + most_novel_action_score = action_score + most_novel_action = min_action + + return self.actionspace(most_novel_action, state) \ No newline at end of file diff --git a/src/examples/gpr.py b/src/examples/gpr.py new file mode 100644 index 00000000..f974d647 --- /dev/null +++ b/src/examples/gpr.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python3 + +from sklearn.gaussian_process import GaussianProcessRegressor +from sklearn.gaussian_process.kernels import RBF +import _pickle as pickle +import numpy as np + +class GuassianProcressRegressor(object): + def __init__(self, kernel=None, scalar=None): + self.kernel = kernel + self.scalar = scalar + self.regressor = GaussianProcessRegressor( + normalize_y=True, + n_restarts_optimizer = 0, + alpha=0.0001, + kernel = RBF(length_scale=0.1)) + + # Update regressor for (distance, angle) given data + # \param X Input features (assumption: 2D matrix shaped n x 3) + # \param Y Input labels (assumption: 1D vector of length n) + # X, Y need to match in quantity + def update_model(self, X, Y, kernel=None): + assert(X.shape[0] == Y.shape[0]) + if self.scalar: + scaler = StandardScaler() + X = scaler.fit_transform(X) + if self.kernel=='rbf' or self.kernel == 'RBF': + rbf_feature = RBFSampler(gamma=1, random_state=1) + X = rbf_feature.fit_transform(X) + self.regressor = self.regressor.fit(X, Y) + + def save_models(self, directory, model_num): + filename = directory + 'model_' + str(model_num) + '.pkl' + pickle.dump(self.regressor, open(filename, 'wb')) + + def load_models(self, directory, model_num): + filename = directory + 'model_' + str(model_num) + '.pkl' + self.regressor = pickle.load(open(filename, 'rb')) + + # Predicts delta state given input + # \param X the action to predict change in state + def predict(self, X): + if self.scalar: + scaler = StandardScaler() + X = scaler.fit_transform(X) + if self.kernel=='rbf' or self.kernel == 'RBF': + rbf_feature = RBFSampler(gamma=1, random_state=1) + X = rbf_feature.fit_transform(X) + return self.regressor.predict(X) + + def uncertainty(self, X_predict): + prediction, std = self.regressor.predict(X_predict, return_std = True) + return prediction, std \ No newline at end of file