From 99d26df49d1fed28467ac5728a852e4f3ee15e6c Mon Sep 17 00:00:00 2001 From: Eric Pan Date: Fri, 20 Sep 2019 16:53:01 -0700 Subject: [PATCH 01/15] remove extraneous files --- CMakeLists.txt | 3 -- include/cozmo_description/cozmo.hpp | 2 - objects/custom_object.py | 77 ----------------------------- src/cozmo_description/cozmo.cpp | 8 --- src/cozmopy/cozmopy.cpp | 18 ++----- 5 files changed, 4 insertions(+), 104 deletions(-) delete mode 100755 objects/custom_object.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 598bcfe8..0cc03e06 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,9 +3,6 @@ project(libcozmo) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") set(CMAKE_BUILD_TYPE Release) -set(PYBIND11_PYTHON_VERSION 3.5) -find_package(PythonInterp 3.5 REQUIRED) -find_package(PythonLibs 3.5 REQUIRED) find_package(catkin REQUIRED COMPONENTS roscpp diff --git a/include/cozmo_description/cozmo.hpp b/include/cozmo_description/cozmo.hpp index 4a3d2dd6..f35d5b82 100644 --- a/include/cozmo_description/cozmo.hpp +++ b/include/cozmo_description/cozmo.hpp @@ -51,8 +51,6 @@ class Cozmo void executeTrajectory(std::chrono::milliseconds period, TrajectoryPtr traj); - void setState(const double& x, const double& y, const Eigen::Quaterniond& orientation); - /// Return and SE2 State defined by the inputted x, y and theta /// \param x The x coordinate of the state diff --git a/objects/custom_object.py b/objects/custom_object.py deleted file mode 100755 index f1087026..00000000 --- a/objects/custom_object.py +++ /dev/null @@ -1,77 +0,0 @@ -#!/usr/bin/env python3 -from cozmo.util import angle_z_to_quaternion, radians - -from visualization_msgs.msg import Marker - -class CustomObject(): - """ - An object class that stores information about a custom rectangular object - """ - def __init__(self, pose, length, width): - """ - Parameters - ---------- - pose : (x, y, theta) of the object in (mm, mm, radians) - length : long side of the object in mm - width : short side of the object in mm - """ - self.pose = pose - self.length = length - self.width = width - - def publish_object(self, pub): - """ - Publishes the object as a cube Marker - """ - box_marker = Marker() - box_marker.header.frame_id = "base_link" - box_marker.type = Marker.CUBE - - box_marker.pose.position.x = self.pose[0] / 1000.0 - box_marker.pose.position.y = self.pose[1] / 1000.0 - box_marker.pose.position.z = 35 / 1000.0 - - box_orientation = angle_z_to_quaternion(radians(self.pose[2])) - box_marker.pose.orientation.x = box_orientation[1] - box_marker.pose.orientation.y = box_orientation[2] - box_marker.pose.orientation.z = box_orientation[3] - box_marker.pose.orientation.w = box_orientation[0] - - box_marker.scale.x = self.width / 1000.0 - box_marker.scale.y = self.length / 1000.0 - box_marker.scale.z = self.width / 1000.0 - box_marker.color.r = 0.0 - box_marker.color.g = 0.5 - box_marker.color.b = 0.5 - box_marker.color.a = 1.0 - - pub.publish(box_marker) - - def __str__(self): - return "Pose: %s, Length: %s, Width: %s" % \ - (self.pose, self.length, self.width) - - def __repr__(self): - return "CustumObject(%s, %s, %s)" % (self.pose, self.length, self.width) - -def create_custom_object(cubes, cube_len): - """ - Creates a CustomObject from given cubes and cube length - Assumes cubes are attached - - Parameters - ---------- - cubes : list containing two LightCube objects - cube_len : length of cube side, in mm - - returns a CustomObject - """ - x1 = cubes[0].pose.position.x - y1 = cubes[0].pose.position.y - x2 = cubes[1].pose.position.x - y2 = cubes[1].pose.position.y - heading = cubes[0].pose.rotation.angle_z.radians - - return CustomObject(((x1+ x2) / 2, - (y1 + y2) / 2, heading), - 2 * cube_len, cube_len) diff --git a/src/cozmo_description/cozmo.cpp b/src/cozmo_description/cozmo.cpp index 52bb279f..8df57f90 100644 --- a/src/cozmo_description/cozmo.cpp +++ b/src/cozmo_description/cozmo.cpp @@ -224,14 +224,6 @@ void Cozmo::executeTrajectory( } } -void Cozmo::setState(const double& x, const double& y, const Eigen::Quaterniond& orientation) { - Eigen::Isometry3d state = Eigen::Isometry3d::Identity(); - state.linear() = orientation.normalized().toRotationMatrix(); - state.translation() << x, y, 0.; - base->getParentJoint()->setPositions( - dart::dynamics::FreeJoint::convertToPositions(state)); -} - SE2::State Cozmo::createState(const double x, const double y, const double th) { SE2::State s; diff --git a/src/cozmopy/cozmopy.cpp b/src/cozmopy/cozmopy.cpp index 552eb768..8e14579b 100644 --- a/src/cozmopy/cozmopy.cpp +++ b/src/cozmopy/cozmopy.cpp @@ -3,7 +3,6 @@ #include #include #include -#include namespace py = pybind11; @@ -29,18 +28,9 @@ PYBIND11_MODULE(cozmopy, m) { return cozmo.executeTrajectory( std::chrono::duration_cast(milliseconds), traj); - }) - .def("setState", []( - libcozmo::Cozmo& cozmo, - const double& x, - const double& y, - const std::vector& quat) - { - Eigen::Quaterniond q(quat[0], quat[1], quat[2], quat[3]); - cozmo.setState(x, y, q); - }); - - py::class_(m, "Waypoint") + }); + + py::class_(m, "Waypoint") .def(py::init([](const double x, const double y, const double th, const double t) { libcozmo::Waypoint w = {.x = x, .y = y, .th = th, .t = t}; return w; @@ -48,4 +38,4 @@ PYBIND11_MODULE(cozmopy, m) } } // namespace python -} // namesapce libcozmo +} // namesapce libcozmo \ No newline at end of file From e06d3cbf11987907f7c5148e0f19d29eed6a220e Mon Sep 17 00:00:00 2001 From: Eric Pan Date: Fri, 20 Sep 2019 17:34:16 -0700 Subject: [PATCH 02/15] making arguments on same line --- objects/run_cozmo.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/objects/run_cozmo.py b/objects/run_cozmo.py index 7ec18b6f..c3475f0d 100755 --- a/objects/run_cozmo.py +++ b/objects/run_cozmo.py @@ -21,10 +21,7 @@ def look_for_object(robot: cozmo.robot): look_around = robot.start_behavior( cozmo.behavior.BehaviorTypes.LookAroundInPlace) try: - cubes = robot.world.wait_until_observe_num_objects( - 2, - cozmo.objects.LightCube, - timeout=10) + cubes = robot.world.wait_until_observe_num_objects(2, cozmo.objects.LightCube, timeout=10) except asyncio.TimeoutError: print('Not enough cubes found') finally: From eea5bcc7282559e1bf34f0bc1cbc452e2452ef1c Mon Sep 17 00:00:00 2001 From: Eric Pan Date: Mon, 23 Sep 2019 17:19:28 -0700 Subject: [PATCH 03/15] update script to incorporate new rectangularcuboid object --- objects/custom_object.py | 85 ++++++++++++++++++++++++++++++++++++++++ objects/run_cozmo.py | 8 ++-- 2 files changed, 90 insertions(+), 3 deletions(-) create mode 100755 objects/custom_object.py diff --git a/objects/custom_object.py b/objects/custom_object.py new file mode 100755 index 00000000..1a72fad7 --- /dev/null +++ b/objects/custom_object.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python3 +from cozmo.util import angle_z_to_quaternion, radians +from visualization_msgs.msg import Marker + +class RectangularCuboid(object): + """ + An object class that stores information about a custom rectangular object + This object is composed of multiple cubes attached horizontally + """ + def __init__(self, num_cubes, pose, side_length): + """ + Parameters + ---------- + num_cubes : int + number of cubes that form this object + pose : (x, y, theta) of the object in (mm, mm, radians) + side_length : float + length of a side of the cube, in mm + """ + self.pose = pose + self.length = num_cubes * side_length + self.width = side_length + self.height = side_length + + def update_object(self, cubes): + """ + Updates the object's pose given the cubes + Assumes cubes are attached horizontally + + Parameters + ---------- + cubes : list containing LightCube objects + """ + x_positions = [] + y_positions = [] + + for cube in cubes: + x_positions.append(cube.pose.position.x) + y_positions.append(cube.pose.position.y) + + heading = cubes[0].pose.rotation.angle_z.radians + + self.pose = (sum(x_positions) / len(cubes), sum(y_positions) / len(cubes), heading) + self.length = len(cubes) * self.width + + def publish_cube(self, publisher, color=(0, 0.5, 0.5, 1)): + """ + Publishes the object as a cube Marker + + Parameters + --------- + publisher : ros publisher + 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 + + 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] + + publisher.publish(cube_marker) + + def __str__(self): + return "Pose: %s, Length: %s, Width: %s" % \ + (self.pose, self.length, self.width) + + def __repr__(self): + return "RectangularCuboid(%s, %s, %s)" % (self.pose, self.length, self.width) diff --git a/objects/run_cozmo.py b/objects/run_cozmo.py index c3475f0d..20ff157a 100755 --- a/objects/run_cozmo.py +++ b/objects/run_cozmo.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -from custom_object import CustomObject, create_custom_object +from custom_object import RectangularCuboid import rospy import cozmo @@ -26,7 +26,9 @@ def look_for_object(robot: cozmo.robot): print('Not enough cubes found') finally: look_around.stop() - return create_custom_object(cubes, 45) + cube = RectangularCuboid(2, (0, 0, 0), 45) + cube.update_object(cubes) + return cube def publish_cozmo(pub, robot, cozmo): """ @@ -63,7 +65,7 @@ def cozmo_run(robot: cozmo.robot): while not rospy.is_shutdown(): custom_obj = look_for_object(robot) - custom_obj.publish_object(object_publisher) + custom_obj.publish_cube(object_publisher) rospy.sleep(0.001) # Actions to take From 77cfbe1a0182e02b2f02f4e85184cabcb2982789 Mon Sep 17 00:00:00 2001 From: Eric Pan Date: Mon, 23 Sep 2019 17:21:15 -0700 Subject: [PATCH 04/15] remove custom_object --- objects/custom_object.py | 85 ---------------------------------------- 1 file changed, 85 deletions(-) delete mode 100755 objects/custom_object.py diff --git a/objects/custom_object.py b/objects/custom_object.py deleted file mode 100755 index 1a72fad7..00000000 --- a/objects/custom_object.py +++ /dev/null @@ -1,85 +0,0 @@ -#!/usr/bin/env python3 -from cozmo.util import angle_z_to_quaternion, radians -from visualization_msgs.msg import Marker - -class RectangularCuboid(object): - """ - An object class that stores information about a custom rectangular object - This object is composed of multiple cubes attached horizontally - """ - def __init__(self, num_cubes, pose, side_length): - """ - Parameters - ---------- - num_cubes : int - number of cubes that form this object - pose : (x, y, theta) of the object in (mm, mm, radians) - side_length : float - length of a side of the cube, in mm - """ - self.pose = pose - self.length = num_cubes * side_length - self.width = side_length - self.height = side_length - - def update_object(self, cubes): - """ - Updates the object's pose given the cubes - Assumes cubes are attached horizontally - - Parameters - ---------- - cubes : list containing LightCube objects - """ - x_positions = [] - y_positions = [] - - for cube in cubes: - x_positions.append(cube.pose.position.x) - y_positions.append(cube.pose.position.y) - - heading = cubes[0].pose.rotation.angle_z.radians - - self.pose = (sum(x_positions) / len(cubes), sum(y_positions) / len(cubes), heading) - self.length = len(cubes) * self.width - - def publish_cube(self, publisher, color=(0, 0.5, 0.5, 1)): - """ - Publishes the object as a cube Marker - - Parameters - --------- - publisher : ros publisher - 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 - - 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] - - publisher.publish(cube_marker) - - def __str__(self): - return "Pose: %s, Length: %s, Width: %s" % \ - (self.pose, self.length, self.width) - - def __repr__(self): - return "RectangularCuboid(%s, %s, %s)" % (self.pose, self.length, self.width) From 403ee881e129ff9125697a11071514d23a08f857 Mon Sep 17 00:00:00 2001 From: Eric Pan Date: Wed, 25 Sep 2019 17:50:42 -0700 Subject: [PATCH 05/15] playing around with offsets to make cozmo be in line with box --- objects/run_cozmo.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/objects/run_cozmo.py b/objects/run_cozmo.py index 20ff157a..f8e22673 100755 --- a/objects/run_cozmo.py +++ b/objects/run_cozmo.py @@ -38,8 +38,8 @@ def publish_cozmo(pub, robot, cozmo): 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, + (robot.pose.position.x - 45) / 999.0, + (robot.pose.position.y + 55 * 2) / 1000.0, quat) rospy.sleep(0.001) From 4c0e5925b310a014f7ac1ca6454559ac72a1012b Mon Sep 17 00:00:00 2001 From: Vinitha Ranganeni Date: Wed, 25 Sep 2019 18:49:37 -0700 Subject: [PATCH 06/15] began adding python bindings for object oriented action space --- src/cozmopy/cozmopy.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/cozmopy/cozmopy.cpp b/src/cozmopy/cozmopy.cpp index 552eb768..31b08377 100644 --- a/src/cozmopy/cozmopy.cpp +++ b/src/cozmopy/cozmopy.cpp @@ -2,6 +2,7 @@ #include #include #include +#include "actionspace/ObjectOrientedActionSpace.hpp" #include #include @@ -45,6 +46,13 @@ 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&>()); } } // namespace python From 6ff824ebc58f6d8186899858fece90dd0d44561b Mon Sep 17 00:00:00 2001 From: Eric Pan Date: Wed, 25 Sep 2019 19:29:15 -0700 Subject: [PATCH 07/15] working exploration with table edge detection, cozmo moving and pushing object, and all visualized in rviz --- nodes/edge_detection.py | 13 ++++-- objects/run_cozmo.py => nodes/explore.py | 54 ++++++++++++++++++------ 2 files changed, 51 insertions(+), 16 deletions(-) rename objects/run_cozmo.py => nodes/explore.py (56%) diff --git a/nodes/edge_detection.py b/nodes/edge_detection.py index e0ca4539..050df672 100755 --- a/nodes/edge_detection.py +++ b/nodes/edge_detection.py @@ -50,6 +50,10 @@ 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): """ @@ -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/objects/run_cozmo.py b/nodes/explore.py similarity index 56% rename from objects/run_cozmo.py rename to nodes/explore.py index f8e22673..58ee5dd0 100755 --- a/objects/run_cozmo.py +++ b/nodes/explore.py @@ -1,4 +1,7 @@ #!/usr/bin/env python3 +import sys +sys.path.append('../objects') +from edge_detection import CornerDetector from custom_object import RectangularCuboid import rospy @@ -13,19 +16,26 @@ from aikidopy import InteractiveMarkerViewer -def look_for_object(robot: cozmo.robot): +def look_for_object(robot: cozmo.robot, timeout=2): """ Tells Cozmo to look around for two LightCubes and constructs a CustomObject representing them """ - look_around = robot.start_behavior( - cozmo.behavior.BehaviorTypes.LookAroundInPlace) - try: - cubes = robot.world.wait_until_observe_num_objects(2, cozmo.objects.LightCube, timeout=10) - except asyncio.TimeoutError: - print('Not enough cubes found') - finally: - look_around.stop() + 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() + input("Cozmo can't find any cubes, please reset cozmo and cubes and then press Enter to continue") + except asyncio.TimeoutError: + continue + finally: + look_around.stop() cube = RectangularCuboid(2, (0, 0, 0), 45) cube.update_object(cubes) return cube @@ -39,7 +49,7 @@ def publish_cozmo(pub, robot, cozmo): quat = [q.q0, q.q1, q.q2, q.q3] cozmo.setState( (robot.pose.position.x - 45) / 999.0, - (robot.pose.position.y + 55 * 2) / 1000.0, + (robot.pose.position.y + 60) / 1000.0, quat) rospy.sleep(0.001) @@ -57,23 +67,43 @@ def cozmo_run(robot: cozmo.robot): 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) 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) + input('Reset Cozmo to the bottom left of the surface for alignment') + while not rospy.is_shutdown(): - custom_obj = look_for_object(robot) + #detector.publish_plane(plane_publisher, 0, color=(1, 0.5, 0)) + #detector.publish_plane(boundary_publisher, 2, True) + + custom_obj = look_for_object(robot, look_for_duration) custom_obj.publish_cube(object_publisher) rospy.sleep(0.001) - # Actions to take + # Part 1 of oo action: getting to object robot.go_to_pose(pose_z_angle( custom_obj.pose[0], custom_obj.pose[1], 0, radians(custom_obj.pose[2]))).wait_for_completed() + # Part 2 of oo action: pushing object + # robot.drive_wheels(speed, speed, duration=duration) + + x_coord = robot.pose.position.x + y_coord = robot.pose.position.y + + # check if cozmo is 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() From e8142185298524bcb2631bd781e1d64330355589 Mon Sep 17 00:00:00 2001 From: Eric Pan Date: Wed, 25 Sep 2019 19:34:43 -0700 Subject: [PATCH 08/15] within bounds check --- nodes/explore.py | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/nodes/explore.py b/nodes/explore.py index 58ee5dd0..fa50cf73 100755 --- a/nodes/explore.py +++ b/nodes/explore.py @@ -77,12 +77,12 @@ def cozmo_run(robot: cozmo.robot): look_for_duration = float(input('How long to look for cubes? ')) threshold = float(input('Boundary threshold? ')) - #detector = CornerDetector(robot, threshold) + detector = CornerDetector(robot, threshold) input('Reset Cozmo to the bottom left of the surface for alignment') while not rospy.is_shutdown(): - #detector.publish_plane(plane_publisher, 0, color=(1, 0.5, 0)) - #detector.publish_plane(boundary_publisher, 2, True) + detector.publish_plane(plane_publisher, 0, color=(1, 0.5, 0)) + detector.publish_plane(boundary_publisher, 2, True) custom_obj = look_for_object(robot, look_for_duration) custom_obj.publish_cube(object_publisher) @@ -101,8 +101,14 @@ def cozmo_run(robot: cozmo.robot): y_coord = robot.pose.position.y # check if cozmo is within bounds: - + if not detector.within_bounds(x_coord, y_coord): + print(detector.y_max, detector.x_max) + print(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), From 4fecacbcc0ff34d59488acb04f9c4c83e0a9e542 Mon Sep 17 00:00:00 2001 From: Vinitha Ranganeni Date: Thu, 26 Sep 2019 00:32:18 -0700 Subject: [PATCH 09/15] ObjectOrientedActionSpace python bindings; python code for gpr and exploration strategies --- src/cozmopy/cozmopy.cpp | 41 ++++++++++++++++++-- src/examples/exploration_strategies.py | 17 +++++++++ src/examples/gpr.py | 53 ++++++++++++++++++++++++++ 3 files changed, 108 insertions(+), 3 deletions(-) create mode 100644 src/examples/exploration_strategies.py create mode 100644 src/examples/gpr.py diff --git a/src/cozmopy/cozmopy.cpp b/src/cozmopy/cozmopy.cpp index 31b08377..e487e3cf 100644 --- a/src/cozmopy/cozmopy.cpp +++ b/src/cozmopy/cozmopy.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include #include "actionspace/ObjectOrientedActionSpace.hpp" #include @@ -47,12 +48,46 @@ PYBIND11_MODULE(cozmopy, m) return w; })); - py::class_(m, "ObjectOrientedActionSpace") - .def(py::init&, + py::class_(m, "ObjectOrientedActionSpace") + .def(py::init&, const std::vector&, const Eigen::Vector2d&, const Eigen::Vector2d&, - const int&>()); + const int&>()) + .def("action_similarity", + &actionspace::ObjectOrientedActionSpace::action_similarity, + py::arg("action_id1"), + py::arg("action_id2"), + py::arg("similarity")) + .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", + &actionspace::ObjectOrientedActionSpace::get_generic_to_object_oriented_action, + py::arg("action_id"), + py::arg("_state"), + py::arg("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..1e8601e4 --- /dev/null +++ b/src/examples/exploration_strategies.py @@ -0,0 +1,17 @@ +#!/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): + return random.choice([i for i in range(self.actionspace.size())]) + + def \ 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 From 4d0673c6b62b57ae03c8e3067f4c20ccd0fccd38 Mon Sep 17 00:00:00 2001 From: Vinitha Ranganeni Date: Thu, 26 Sep 2019 01:16:57 -0700 Subject: [PATCH 10/15] minor edits --- src/examples/exploration_strategies.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/examples/exploration_strategies.py b/src/examples/exploration_strategies.py index 1e8601e4..78bb27d5 100644 --- a/src/examples/exploration_strategies.py +++ b/src/examples/exploration_strategies.py @@ -14,4 +14,8 @@ def __init__(actionspace): def action(self): return random.choice([i for i in range(self.actionspace.size())]) - def \ No newline at end of file +class Novelty: + def __init__(actionspace): + self.actionspace = actionspace + + \ No newline at end of file From 42bbb72a03e802bf083d4a36d5c7cd0a54a58105 Mon Sep 17 00:00:00 2001 From: Vinitha Ranganeni Date: Thu, 26 Sep 2019 11:09:10 -0700 Subject: [PATCH 11/15] minor edits to object oriented action space python bindings --- src/cozmopy/cozmopy.cpp | 29 ++++++++++++++++--------- src/examples/exploration_strategies.py | 30 +++++++++++++++++++++++++- 2 files changed, 48 insertions(+), 11 deletions(-) diff --git a/src/cozmopy/cozmopy.cpp b/src/cozmopy/cozmopy.cpp index e487e3cf..edb52637 100644 --- a/src/cozmopy/cozmopy.cpp +++ b/src/cozmopy/cozmopy.cpp @@ -54,22 +54,31 @@ PYBIND11_MODULE(cozmopy, m) const Eigen::Vector2d&, const Eigen::Vector2d&, const int&>()) - .def("action_similarity", - &actionspace::ObjectOrientedActionSpace::action_similarity, - py::arg("action_id1"), - py::arg("action_id2"), - py::arg("similarity")) + .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", - &actionspace::ObjectOrientedActionSpace::get_generic_to_object_oriented_action, - py::arg("action_id"), - py::arg("_state"), - py::arg("action")) + .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, successful; + }) .def("publish_action", &actionspace::ObjectOrientedActionSpace::publish_action, py::arg("action_id"), diff --git a/src/examples/exploration_strategies.py b/src/examples/exploration_strategies.py index 78bb27d5..a53dd4ec 100644 --- a/src/examples/exploration_strategies.py +++ b/src/examples/exploration_strategies.py @@ -17,5 +17,33 @@ def action(self): 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())] - \ No newline at end of file + 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): + 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(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.model_action_to_action(most_novel_action) \ No newline at end of file From b4b35d1e752d5d09ffabc5bcd244b5bbdb527007 Mon Sep 17 00:00:00 2001 From: Vinitha Ranganeni Date: Thu, 26 Sep 2019 11:09:10 -0700 Subject: [PATCH 12/15] minor edits to object oriented action space python bindings --- src/cozmopy/cozmopy.cpp | 29 +++++++++++++++-------- src/examples/exploration_strategies.py | 32 ++++++++++++++++++++++++-- 2 files changed, 49 insertions(+), 12 deletions(-) diff --git a/src/cozmopy/cozmopy.cpp b/src/cozmopy/cozmopy.cpp index e487e3cf..edb52637 100644 --- a/src/cozmopy/cozmopy.cpp +++ b/src/cozmopy/cozmopy.cpp @@ -54,22 +54,31 @@ PYBIND11_MODULE(cozmopy, m) const Eigen::Vector2d&, const Eigen::Vector2d&, const int&>()) - .def("action_similarity", - &actionspace::ObjectOrientedActionSpace::action_similarity, - py::arg("action_id1"), - py::arg("action_id2"), - py::arg("similarity")) + .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", - &actionspace::ObjectOrientedActionSpace::get_generic_to_object_oriented_action, - py::arg("action_id"), - py::arg("_state"), - py::arg("action")) + .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, successful; + }) .def("publish_action", &actionspace::ObjectOrientedActionSpace::publish_action, py::arg("action_id"), diff --git a/src/examples/exploration_strategies.py b/src/examples/exploration_strategies.py index 78bb27d5..735631b0 100644 --- a/src/examples/exploration_strategies.py +++ b/src/examples/exploration_strategies.py @@ -11,11 +11,39 @@ class Random: def __init__(actionspace): self.actionspace = actionspace - def action(self): + 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())] - \ No newline at end of file + 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 From 0c94c86d23e2cf77a29e13faa06fc01f792a9c67 Mon Sep 17 00:00:00 2001 From: Eric Pan Date: Thu, 26 Sep 2019 13:54:44 -0700 Subject: [PATCH 13/15] Mid debugging rviz and cozmo moving --- nodes/edge_detection.py | 8 +-- nodes/explore.py | 123 +++++++++++++++++++++++++++------------ objects/custom_object.py | 42 ++++++------- src/cozmopy/cozmopy.cpp | 2 +- 4 files changed, 113 insertions(+), 62 deletions(-) diff --git a/nodes/edge_detection.py b/nodes/edge_detection.py index 050df672..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 @@ -59,8 +59,8 @@ 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)): """ diff --git a/nodes/explore.py b/nodes/explore.py index fa50cf73..752788f3 100755 --- a/nodes/explore.py +++ b/nodes/explore.py @@ -6,17 +6,19 @@ import rospy import cozmo -from cozmo.util import angle_z_to_quaternion, pose_z_angle, radians +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 +from cozmopy import Cozmo, ObjectOrientedActionSpace from aikidopy import InteractiveMarkerViewer +import random -def look_for_object(robot: cozmo.robot, timeout=2): + +def look_for_object(robot: cozmo.robot, cube, timeout=2): """ Tells Cozmo to look around for two LightCubes and constructs a CustomObject representing them @@ -31,14 +33,16 @@ def look_for_object(robot: cozmo.robot, timeout=2): done = True else: look_around.stop() - input("Cozmo can't find any cubes, please reset cozmo and cubes and then press Enter to continue") + 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 cube + # cube = RectangularCuboid(2, (0, 0, 0), 45) + # cube.update_object(cubes) + return cubes def publish_cozmo(pub, robot, cozmo): """ @@ -48,7 +52,7 @@ def publish_cozmo(pub, robot, cozmo): q = robot.pose.rotation quat = [q.q0, q.q1, q.q2, q.q3] cozmo.setState( - (robot.pose.position.x - 45) / 999.0, + (robot.pose.position.x - 45) / 1000.0, (robot.pose.position.y + 60) / 1000.0, quat) rospy.sleep(0.001) @@ -56,63 +60,108 @@ def publish_cozmo(pub, robot, cozmo): 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) + #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) + # detector.publish_plane(plane_publisher, 0, color=(1, 0.5, 0)) + # detector.publish_plane(boundary_publisher, 2, True) - custom_obj = look_for_object(robot, look_for_duration) - custom_obj.publish_cube(object_publisher) + 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) - # Part 1 of oo action: getting to object - robot.go_to_pose(pose_z_angle( - custom_obj.pose[0], - custom_obj.pose[1], + 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(custom_obj.pose[2]))).wait_for_completed() - # Part 2 of oo action: pushing object - # robot.drive_wheels(speed, speed, duration=duration) + 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 + # x_coord = robot.pose.position.x + # y_coord = robot.pose.position.y - # check if cozmo is within bounds: - if not detector.within_bounds(x_coord, y_coord): - print(detector.y_max, detector.x_max) - print(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() + # 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] + 45) / 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", []) 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/cozmopy/cozmopy.cpp b/src/cozmopy/cozmopy.cpp index edb52637..77c3c36a 100644 --- a/src/cozmopy/cozmopy.cpp +++ b/src/cozmopy/cozmopy.cpp @@ -77,7 +77,7 @@ PYBIND11_MODULE(cozmopy, m) 0.0, Eigen::Vector3d(0, 0, 0)); bool successful = actionspace.get_generic_to_object_oriented_action( action_id, _state, &action); - return action, successful; + return action; }) .def("publish_action", &actionspace::ObjectOrientedActionSpace::publish_action, From 93d738b60db6bdacd32e9033d21a3e304b089cad Mon Sep 17 00:00:00 2001 From: Vinitha Ranganeni Date: Thu, 26 Sep 2019 19:38:39 -0700 Subject: [PATCH 14/15] fixed cozmo's center of mass --- src/cozmo_description/cozmo.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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; From 453f9c99cb401b548d197c022a4dbe94aab56ce1 Mon Sep 17 00:00:00 2001 From: Eric Pan Date: Wed, 2 Oct 2019 16:33:20 -0700 Subject: [PATCH 15/15] Working visualization for plane, cube, cozmo and start position; generated start position not computed correctly --- nodes/explore.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nodes/explore.py b/nodes/explore.py index 752788f3..c79b032e 100755 --- a/nodes/explore.py +++ b/nodes/explore.py @@ -52,8 +52,8 @@ def publish_cozmo(pub, robot, cozmo): q = robot.pose.rotation quat = [q.q0, q.q1, q.q2, q.q3] cozmo.setState( - (robot.pose.position.x - 45) / 1000.0, - (robot.pose.position.y + 60) / 1000.0, + robot.pose.position.x / 1000.0, + robot.pose.position.y / 1000.0, quat) rospy.sleep(0.001) @@ -143,7 +143,7 @@ def publish_cozmo_start(publisher, cozmo_pose, color=(0, 0, 0, 1)): cozmo_marker.type = Marker.CUBE cozmo_marker.pose.position.x = (cozmo_pose[0]) / 1000 - cozmo_marker.pose.position.y = (cozmo_pose[1] + 45) / 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]