From 57164dc946f1da21525ac833509aeb730ac55165 Mon Sep 17 00:00:00 2001 From: Eric Pan Date: Wed, 2 Oct 2019 17:09:38 -0700 Subject: [PATCH 1/5] starting path finder script for cozmo --- nodes/path_finder.py | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100755 nodes/path_finder.py diff --git a/nodes/path_finder.py b/nodes/path_finder.py new file mode 100755 index 00000000..cc1099ae --- /dev/null +++ b/nodes/path_finder.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python3 +import rospy + +import cozmo + +class PathFinder(object): + + def __init__(self, robot): + self.robot = robot + + def find_path(self, steps, goal): + """ + steps : int + how many steps to take to get to goal + """ + + def move_forward(self, speed=50, duration=1): + robot.drive_wheels(speed, speed, duration=duration) + + def rotate(self, angle): + """ + Turns the robot + """ + robot.turn_in_place(degrees(angle)).wait_for_completed() + + +def cozmo_run(robot: cozmo.robot): + path_finder = PathFinder(robot) + + + +if __name__ == '__main__': + rospy.init_node('path_finder') + try: + cozmo.run_program(cozmo_run) + except rospy.ROSInterruptException: + pass From 6a984a32c7a5bf53b02a2ca16e27f21c9d11245f Mon Sep 17 00:00:00 2001 From: Eric Pan Date: Thu, 3 Oct 2019 17:54:35 -0700 Subject: [PATCH 2/5] Just need to access waypoint attributes and this should work; need to compare go_to_pose with drive_wheels and rotate_in_place --- nodes/path_finder.py | 61 +++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 60 insertions(+), 1 deletion(-) diff --git a/nodes/path_finder.py b/nodes/path_finder.py index cc1099ae..5eb0875b 100755 --- a/nodes/path_finder.py +++ b/nodes/path_finder.py @@ -2,6 +2,14 @@ import rospy import cozmo +from cozmo.util import degrees, pose_z_angle, radians +import threading +from math import pi +from datetime import timedelta + +from roscpp_initializer import roscpp_initializer +from cozmopy import Cozmo, Waypoint +from aikidopy import InteractiveMarkerViewer class PathFinder(object): @@ -23,15 +31,66 @@ def rotate(self, angle): """ robot.turn_in_place(degrees(angle)).wait_for_completed() +def update_cozmo(robot, cozmo): + """ + Thread to update cozmo's location + """ + 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, + robot.pose.position.y / 1000, + quat) + rospy.sleep(0.001) def cozmo_run(robot: cozmo.robot): - path_finder = PathFinder(robot) + topicName = "cozmo_model" + baseFrameName = "base_link" + + if not rospy.is_shutdown(): + cozmo = Cozmo("/home/bubbletea/cozmo_ws/src/libcozmo/src/cozmo_description/meshes") + skeleton = cozmo.getCozmoSkeleton() + + viewer = InteractiveMarkerViewer(topicName, baseFrameName) + cozmo_marker = viewer.addSkeleton(skeleton) + viewer.setAutoUpdate(True) + + t = threading.Thread( + target = update_cozmo, + args=(robot, cozmo)) + t.start() + + waypoints = [ + Waypoint(0.0, 0.0, 0, 0), + Waypoint(0.3, 0.0, 0, 2), + Waypoint(0.3, 0.0, pi/2, 3), + Waypoint(0.3, 0.3, pi/2, 5), + Waypoint(0.3, 0.3, pi, 6), + Waypoint(0.0, 0.3, pi, 8), + Waypoint(0.0, 0.3, -pi/2, 9), + Waypoint(0.0, 0.0, -pi/2, 11), + Waypoint(0.0, 0.0, 0, 12), + ] + + #traj = cozmo.createInterpolatedTraj(waypoints); + # cozmo.executeTrajectory(timedelta(milliseconds=6), traj) + + for waypoint in waypoints: + print([method_name for method_name in dir(waypoint) + if callable(getattr(waypoint, method_name))]) + robot.go_to_pose(pose_z_angle(waypoint[0], waypoint[1], 0, radians(waypoint[2]))).wait_for_completed() + #path_finder = PathFinder(robot) if __name__ == '__main__': + roscpp_initializer.roscpp_init("path_finder", []) rospy.init_node('path_finder') try: cozmo.run_program(cozmo_run) except rospy.ROSInterruptException: pass + +# Could potentially use drive_wheels, rotate_in_place and calculate waypoint movement instead of go_to_pose, +# but need to test which is better From 8bee487bd994c5232a0531f5b227e024919150a6 Mon Sep 17 00:00:00 2001 From: Eric Pan Date: Mon, 7 Oct 2019 18:10:22 -0700 Subject: [PATCH 3/5] trying to figure out moving cozmo and visualizing at same time with respect to planner --- nodes/path_finder.py | 40 +++++++++++++++++++++++++++++++++------- 1 file changed, 33 insertions(+), 7 deletions(-) diff --git a/nodes/path_finder.py b/nodes/path_finder.py index 5eb0875b..6d313fd7 100755 --- a/nodes/path_finder.py +++ b/nodes/path_finder.py @@ -4,6 +4,7 @@ import cozmo from cozmo.util import degrees, pose_z_angle, radians import threading +import math from math import pi from datetime import timedelta @@ -61,8 +62,28 @@ def cozmo_run(robot: cozmo.robot): args=(robot, cozmo)) t.start() + # path = [action1_id, action2_id] + # for action in path: + # GenericAction a = Actionspace.get_action(action) + + # Create a path that is in the shape of a j + speed = 100 + duration = 1 + headings = [0, 0, 3 * pi / 2, pi] + + current_pos = (0, 0, 0) + poses = [] + poses.append(current_pos) + waypoints = [] + waypoints.append(Waypoint(current_pos[0], current_pos[1], current_pos[2], 0)) + + for i in range(len(headings))[1:]: + current_pos = generic_action_to_xytheta(current_pos, speed, duration, headings[i]) + poses.append(current_pos) + waypoints.append(Waypoint(current_pos[0], current_pos[1], current_pos[2], i*2)) + waypoints = [ - Waypoint(0.0, 0.0, 0, 0), + Waypoint(0.0, 0.0, 0, 0), Waypoint(0.3, 0.0, 0, 2), Waypoint(0.3, 0.0, pi/2, 3), Waypoint(0.3, 0.3, pi/2, 5), @@ -73,15 +94,20 @@ def cozmo_run(robot: cozmo.robot): Waypoint(0.0, 0.0, 0, 12), ] - #traj = cozmo.createInterpolatedTraj(waypoints); - # cozmo.executeTrajectory(timedelta(milliseconds=6), traj) + print(waypoints) + print(poses) + traj = cozmo.createInterpolatedTraj(waypoints); + cozmo.executeTrajectory(timedelta(milliseconds=6), traj) - for waypoint in waypoints: - print([method_name for method_name in dir(waypoint) - if callable(getattr(waypoint, method_name))]) - robot.go_to_pose(pose_z_angle(waypoint[0], waypoint[1], 0, radians(waypoint[2]))).wait_for_completed() + #for waypoint in waypoints: + # robot.go_to_pose(pose_z_angle(waypoint[0], waypoint[1], 0, radians(waypoint[2]))).wait_for_completed() #path_finder = PathFinder(robot) +def generic_action_to_xytheta(current_pos, speed, duration, heading): + # x, in mm + return (current_pos[0] + math.cos(heading) * speed * duration, + current_pos[1] + math.sin(heading) * speed * duration, + heading) if __name__ == '__main__': From b342576c36115794c92f58e6627e8f0432d467bc Mon Sep 17 00:00:00 2001 From: Eric Pan Date: Sun, 13 Oct 2019 15:08:53 -0700 Subject: [PATCH 4/5] working comparison of go_to_pose vs drive_wheels/turn_place in rviz and in real life --- nodes/path_finder.py | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/nodes/path_finder.py b/nodes/path_finder.py index 6d313fd7..91e78947 100755 --- a/nodes/path_finder.py +++ b/nodes/path_finder.py @@ -67,40 +67,40 @@ def cozmo_run(robot: cozmo.robot): # GenericAction a = Actionspace.get_action(action) # Create a path that is in the shape of a j - speed = 100 + # speed is in m/s + speed = 0.1 duration = 1 headings = [0, 0, 3 * pi / 2, pi] + # if heading is different from previous: add to waypoint / pose current_pos = (0, 0, 0) poses = [] poses.append(current_pos) waypoints = [] waypoints.append(Waypoint(current_pos[0], current_pos[1], current_pos[2], 0)) - for i in range(len(headings))[1:]: + for i in range(len(headings)): + if headings[i] != 0: + poses.append((current_pos[0], current_pos[1], headings[i])) + waypoints.append(Waypoint(current_pos[0], current_pos[1], headings[i], i*2 -1)) current_pos = generic_action_to_xytheta(current_pos, speed, duration, headings[i]) poses.append(current_pos) + print('here', poses) waypoints.append(Waypoint(current_pos[0], current_pos[1], current_pos[2], i*2)) - waypoints = [ - Waypoint(0.0, 0.0, 0, 0), - Waypoint(0.3, 0.0, 0, 2), - Waypoint(0.3, 0.0, pi/2, 3), - Waypoint(0.3, 0.3, pi/2, 5), - Waypoint(0.3, 0.3, pi, 6), - Waypoint(0.0, 0.3, pi, 8), - Waypoint(0.0, 0.3, -pi/2, 9), - Waypoint(0.0, 0.0, -pi/2, 11), - Waypoint(0.0, 0.0, 0, 12), - ] - print(waypoints) print(poses) traj = cozmo.createInterpolatedTraj(waypoints); cozmo.executeTrajectory(timedelta(milliseconds=6), traj) - #for waypoint in waypoints: - # robot.go_to_pose(pose_z_angle(waypoint[0], waypoint[1], 0, radians(waypoint[2]))).wait_for_completed() + #for i in range(len(headings)): + # if headings[i] != 0: + # robot.turn_in_place(radians(headings[i]), is_absolute=True).wait_for_completed() + # robot.drive_wheels(speed * 1000, speed * 1000, duration=duration) + for pose in poses: + print('going to pose: ', pose[0] * 1000, pose[1] * 1000, pose[2]) + robot.go_to_pose(pose_z_angle(pose[0] * 1000, pose[1] * 1000, 0, radians(pose[2]))).wait_for_completed() + #path_finder = PathFinder(robot) def generic_action_to_xytheta(current_pos, speed, duration, heading): From 05561da7608cd192b9dc45e3b52db07540ecc05b Mon Sep 17 00:00:00 2001 From: Eric Pan Date: Sun, 13 Oct 2019 17:38:05 -0700 Subject: [PATCH 5/5] refactored and place path traversal and rviz into a class --- nodes/path_finder.py | 210 +++++++++++++++++++++++++++++++------------ 1 file changed, 155 insertions(+), 55 deletions(-) diff --git a/nodes/path_finder.py b/nodes/path_finder.py index 91e78947..5890ba7a 100755 --- a/nodes/path_finder.py +++ b/nodes/path_finder.py @@ -1,3 +1,32 @@ +"""///////////////////////////////////////////////////////////////////////////// +// Copyright (c) 2019, Eric Pan, Vinitha Ranganeni +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice +// this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +/////////////////////////////////////////////////////////////////////////////""" + #!/usr/bin/env python3 import rospy @@ -12,25 +41,128 @@ from cozmopy import Cozmo, Waypoint from aikidopy import InteractiveMarkerViewer +class tempAction(object): + """ + Temp Generic Action until pybindings for cpp version is done + """ + def __init__(self, speed, duration, heading): + """ + speed : double, mm/s + duration : double, seconds + heading : double, radians + """ + self.speed = speed + self.duration = duration + self.heading = heading + class PathFinder(object): + """ + Visualizes and executes a path given a list of generic actions + Option to execute actions from cozmo frame or world frame - def __init__(self, robot): + Cozmo frame (relative path traversal): + More control of how each action is executed but more deviation from rviz + World frame (absolute path traversal): + Less control of how each action is executed and much slower execution time, but less deviation from rviz + + """ + def __init__(self, robot, cozmo, path=None): + """ + Initialize a path to visualize and execute + + Parameters + ---------- + robot : cozmo.robot + The cozmo SDK robot handle + cozmo : cozmo skeleton mesh + path : list of GenericActions + waypoints : list of Waypoint objects for visualization + poses : list of poses (x, y, theta) for execution + time : to keep track of waypoint time, (not sure what unit this is) + previous_heading : to keep track of additional actions to take, in radians + This allows for smoother rviz and absolute path traversal, doesn't affect relative path traversal + current_pos : x, y, theta (mm, mm, radians) + + For a given generic action (speed, duration, heading): + robot will turn to face the heading and then travel + """ self.robot = robot + self.cozmo = cozmo + self.path = [] if None else path + self.waypoints = [] + self.poses = [] + self.time = 0 + self.previous_heading = None + self.current_pos = None + self.init_path() + + def init_path(self, start=(0, 0, 0)): + """ + Initialize the path if one is given, otherwise add a starting location for rviz + """ + self.current_pos = start + self.add(tempAction(start[0], start[1], start[2])) + if self.path: + for action in self.path: + self.add(action, True) + + def add(self, action, init=False): + """ + Adds the given action to the list of waypoints and poses for rviz and traversal respectively + Also appends action to the path if this is called outside the init + + action : GenericAction - def find_path(self, steps, goal): """ - steps : int - how many steps to take to get to goal + # Add a turn action + if (self.previous_heading is None or action.heading != self.previous_heading): + self.previous_heading = action.heading + self.waypoints.append(Waypoint(self.current_pos[0] / 1000, self.current_pos[1] / 1000, action.heading, self.time + 1)) + self.poses.append((self.current_pos[0], self.current_pos[1], action.heading)) + self.update_position(action) + self.waypoints.append(Waypoint(self.current_pos[0] / 1000, self.current_pos[1] / 1000, self.current_pos[2], self.time + 2)) + self.poses.append((self.current_pos[0], self.current_pos[1], self.current_pos[2])) + self.time += 2 + if not init: + self.path.append(action) + + def visualize(self): """ + Visualizes the path in rviz + """ + if self.path and not rospy.is_shutdown(): + traj = self.cozmo.createInterpolatedTraj(self.waypoints); + self.cozmo.executeTrajectory(timedelta(milliseconds=6), traj) + else: + print('No path to visualize') - def move_forward(self, speed=50, duration=1): - robot.drive_wheels(speed, speed, duration=duration) + def execute(self, absolute=False): + """ + Execute the path built so far - def rotate(self, angle): + absolute : True to use world frame traversal, false to use relative frame traversal """ - Turns the robot + if self.path: + if absolute: + for pose in self.poses: + print('going to pose: ', pose[0], pose[1], pose[2]) + self.robot.go_to_pose(pose_z_angle(pose[0], pose[1], 0, radians(pose[2]))).wait_for_completed() + else: + for point in self.path: + if point.heading: + self.robot.turn_in_place(radians(point.heading), is_absolute=True).wait_for_completed() + self.robot.drive_wheels(point.speed, point.speed, duration=point.duration) + + def update_position(self, action): """ - robot.turn_in_place(degrees(angle)).wait_for_completed() + Updates the (x, y, theta) in (mm, mm, radians) given a GenericAction + """ + self.current_pos = (self.current_pos[0] + math.cos(action.heading) * action.speed * action.duration, + self.current_pos[1] + math.sin(action.heading) * action.speed * action.duration, + action.heading) + + #TODO: Some kind of path_generation to go around object function + def update_cozmo(robot, cozmo): """ @@ -62,52 +194,23 @@ def cozmo_run(robot: cozmo.robot): args=(robot, cozmo)) t.start() - # path = [action1_id, action2_id] - # for action in path: - # GenericAction a = Actionspace.get_action(action) - - # Create a path that is in the shape of a j - # speed is in m/s - speed = 0.1 + speed = 100 duration = 1 headings = [0, 0, 3 * pi / 2, pi] - # if heading is different from previous: add to waypoint / pose - current_pos = (0, 0, 0) - poses = [] - poses.append(current_pos) - waypoints = [] - waypoints.append(Waypoint(current_pos[0], current_pos[1], current_pos[2], 0)) - - for i in range(len(headings)): - if headings[i] != 0: - poses.append((current_pos[0], current_pos[1], headings[i])) - waypoints.append(Waypoint(current_pos[0], current_pos[1], headings[i], i*2 -1)) - current_pos = generic_action_to_xytheta(current_pos, speed, duration, headings[i]) - poses.append(current_pos) - print('here', poses) - waypoints.append(Waypoint(current_pos[0], current_pos[1], current_pos[2], i*2)) - - print(waypoints) - print(poses) - traj = cozmo.createInterpolatedTraj(waypoints); - cozmo.executeTrajectory(timedelta(milliseconds=6), traj) - - #for i in range(len(headings)): - # if headings[i] != 0: - # robot.turn_in_place(radians(headings[i]), is_absolute=True).wait_for_completed() - # robot.drive_wheels(speed * 1000, speed * 1000, duration=duration) - for pose in poses: - print('going to pose: ', pose[0] * 1000, pose[1] * 1000, pose[2]) - robot.go_to_pose(pose_z_angle(pose[0] * 1000, pose[1] * 1000, 0, radians(pose[2]))).wait_for_completed() - - #path_finder = PathFinder(robot) - -def generic_action_to_xytheta(current_pos, speed, duration, heading): - # x, in mm - return (current_pos[0] + math.cos(heading) * speed * duration, - current_pos[1] + math.sin(heading) * speed * duration, - heading) + actions = [] + + for heading in headings: + actions.append(tempAction(speed, duration, heading)) + + pf = PathFinder(robot, cozmo, actions) + pf.add(tempAction(200, 1, 5)) + pf.execute(True) + #pf.visualize() + + # path = [action1_id, action2_id] + # for action in path: + # GenericAction a = Actionspace.get_action(action) if __name__ == '__main__': @@ -117,6 +220,3 @@ def generic_action_to_xytheta(current_pos, speed, duration, heading): cozmo.run_program(cozmo_run) except rospy.ROSInterruptException: pass - -# Could potentially use drive_wheels, rotate_in_place and calculate waypoint movement instead of go_to_pose, -# but need to test which is better