Skip to content

Commit 7b6fc4b

Browse files
PeterMitranoPeterMitrano
committed
revert other tutorial
1 parent 4c68104 commit 7b6fc4b

File tree

1 file changed

+70
-75
lines changed

1 file changed

+70
-75
lines changed

‎doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py‎

Lines changed: 70 additions & 75 deletions
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@
3232
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
3333
# POSSIBILITY OF SUCH DAMAGE.
3434
#
35-
# Author: Acorn Pooley, Mike Lautman, Peter Mitrano
35+
# Author: Acorn Pooley, Mike Lautman
3636

3737
## BEGIN_SUB_TUTORIAL imports
3838
##
@@ -41,12 +41,14 @@
4141
## and a `RobotCommander`_ class. More on these below. We also import `rospy`_ and some messages that we will use:
4242
##
4343

44+
# Python 2/3 compatibility imports
45+
from __future__ import print_function
46+
from six.moves import input
47+
4448
import sys
4549
import copy
4650
import rospy
47-
from moveit_commander import roscpp_initializer
48-
from moveit.core.conversions import pose_to_list
49-
from moveit.planning_interface import planning_interface
51+
import moveit_commander
5052
import moveit_msgs.msg
5153
import geometry_msgs.msg
5254
try:
@@ -61,38 +63,34 @@ def dist(p, q):
6163
## END_SUB_TUTORIAL
6264

6365

64-
65-
## END_SUB_TUTORIAL
66-
67-
6866
def all_close(goal, actual, tolerance):
69-
"""
70-
Convenience method for testing if the values in two lists are within a tolerance of each other.
71-
For Pose and PoseStamped inputs, the angle between the two quaternions is compared (the angle
72-
between the identical orientations q and -q is calculated correctly).
73-
@param: goal A list of floats, a Pose or a PoseStamped
74-
@param: actual A list of floats, a Pose or a PoseStamped
75-
@param: tolerance A float
76-
@returns: bool
77-
"""
78-
if type(goal) is list:
79-
for index in range(len(goal)):
80-
if abs(actual[index] - goal[index]) > tolerance:
81-
return False
82-
83-
elif type(goal) is geometry_msgs.msg.PoseStamped:
84-
return all_close(goal.pose, actual.pose, tolerance)
85-
86-
elif type(goal) is geometry_msgs.msg.Pose:
87-
x0, y0, z0, qx0, qy0, qz0, qw0 = pose_to_list(actual)
88-
x1, y1, z1, qx1, qy1, qz1, qw1 = pose_to_list(goal)
89-
# Euclidean distance
90-
d = dist((x1, y1, z1), (x0, y0, z0))
91-
# phi = angle between orientations
92-
cos_phi_half = fabs(qx0 * qx1 + qy0 * qy1 + qz0 * qz1 + qw0 * qw1)
93-
return d <= tolerance and cos_phi_half >= cos(tolerance / 2.0)
94-
95-
return True
67+
"""
68+
Convenience method for testing if the values in two lists are within a tolerance of each other.
69+
For Pose and PoseStamped inputs, the angle between the two quaternions is compared (the angle
70+
between the identical orientations q and -q is calculated correctly).
71+
@param: goal A list of floats, a Pose or a PoseStamped
72+
@param: actual A list of floats, a Pose or a PoseStamped
73+
@param: tolerance A float
74+
@returns: bool
75+
"""
76+
if type(goal) is list:
77+
for index in range(len(goal)):
78+
if abs(actual[index] - goal[index]) > tolerance:
79+
return False
80+
81+
elif type(goal) is geometry_msgs.msg.PoseStamped:
82+
return all_close(goal.pose, actual.pose, tolerance)
83+
84+
elif type(goal) is geometry_msgs.msg.Pose:
85+
x0, y0, z0, qx0, qy0, qz0, qw0 = pose_to_list(actual)
86+
x1, y1, z1, qx1, qy1, qz1, qw1 = pose_to_list(goal)
87+
# Euclidean distance
88+
d = dist((x1, y1, z1), (x0, y0, z0))
89+
# phi = angle between orientations
90+
cos_phi_half = fabs(qx0*qx1 + qy0*qy1 + qz0*qz1 + qw0*qw1)
91+
return d <= tolerance and cos_phi_half >= cos(tolerance / 2.0)
92+
93+
return True
9694

9795

9896
class MoveGroupPythonInterfaceTutorial(object):
@@ -177,7 +175,7 @@ def go_to_joint_state(self):
177175
## Planning to a Joint Goal
178176
## ^^^^^^^^^^^^^^^^^^^^^^^^
179177
## The Panda's zero configuration is at a `singularity <https://www.quora.com/Robotics-What-is-meant-by-kinematic-singularity>`_, so the first
180-
## thing we want to do is move it to a slightly better configuration.
178+
## thing we want to do is move it to a slightly better configuration.
181179
## We use the constant `tau = 2*pi <https://en.wikipedia.org/wiki/Turn_(angle)#Tau_proposals>`_ for convenience:
182180
# We get the joint values from the group and change some of the values:
183181
joint_goal = move_group.get_current_joint_values()
@@ -468,58 +466,55 @@ def remove_box(self, timeout=4):
468466

469467

470468
def main():
471-
try:
472-
print("")
473-
print("----------------------------------------------------------")
474-
print("Welcome to the MoveIt MoveGroup Python Interface Tutorial")
475-
print("----------------------------------------------------------")
476-
print("Press Ctrl-D to exit at any time")
477-
print("")
478-
input("============ Press `Enter` to begin the tutorial by setting up the moveit_commander ...")
479-
tutorial = MoveGroupPythonInterfaceTutorial()
480-
481-
input("============ Press `Enter` to execute a movement using a joint state goal ...")
482-
tutorial.go_to_joint_state()
483-
484-
input("============ Press `Enter` to execute a movement using a pose goal ...")
485-
tutorial.go_to_pose_goal()
469+
try:
470+
print("")
471+
print("----------------------------------------------------------")
472+
print("Welcome to the MoveIt MoveGroup Python Interface Tutorial")
473+
print("----------------------------------------------------------")
474+
print("Press Ctrl-D to exit at any time")
475+
print("")
476+
input("============ Press `Enter` to begin the tutorial by setting up the moveit_commander ...")
477+
tutorial = MoveGroupPythonInterfaceTutorial()
486478

487-
input("============ Press `Enter` to plan and display a Cartesian path ...")
488-
cartesian_plan, fraction = tutorial.plan_cartesian_path()
479+
input("============ Press `Enter` to execute a movement using a joint state goal ...")
480+
tutorial.go_to_joint_state()
489481

490-
input("============ Press `Enter` to display a saved trajectory (this will replay the Cartesian path) ...")
491-
tutorial.display_trajectory(cartesian_plan)
482+
input("============ Press `Enter` to execute a movement using a pose goal ...")
483+
tutorial.go_to_pose_goal()
492484

493-
input("============ Press `Enter` to execute a saved path ...")
494-
tutorial.execute_plan(cartesian_plan)
485+
input("============ Press `Enter` to plan and display a Cartesian path ...")
486+
cartesian_plan, fraction = tutorial.plan_cartesian_path()
495487

496-
input("============ Press `Enter` to add a box to the planning scene ...")
497-
tutorial.add_box()
488+
input("============ Press `Enter` to display a saved trajectory (this will replay the Cartesian path) ...")
489+
tutorial.display_trajectory(cartesian_plan)
498490

499-
input("============ Press `Enter` to attach a Box to the Panda robot ...")
500-
tutorial.attach_box()
491+
input("============ Press `Enter` to execute a saved path ...")
492+
tutorial.execute_plan(cartesian_plan)
501493

502-
input("============ Press `Enter` to plan and execute a path with an attached collision object ...")
503-
cartesian_plan, fraction = tutorial.plan_cartesian_path(scale=-1)
504-
tutorial.execute_plan(cartesian_plan)
494+
input("============ Press `Enter` to add a box to the planning scene ...")
495+
tutorial.add_box()
505496

506-
input("============ Press `Enter` to detach the box from the Panda robot ...")
507-
tutorial.detach_box()
497+
input("============ Press `Enter` to attach a Box to the Panda robot ...")
498+
tutorial.attach_box()
508499

509-
input("============ Press `Enter` to remove the box from the planning scene ...")
510-
tutorial.remove_box()
500+
input("============ Press `Enter` to plan and execute a path with an attached collision object ...")
501+
cartesian_plan, fraction = tutorial.plan_cartesian_path(scale=-1)
502+
tutorial.execute_plan(cartesian_plan)
511503

512-
print("============ Python tutorial demo complete!")
504+
input("============ Press `Enter` to detach the box from the Panda robot ...")
505+
tutorial.detach_box()
513506

514-
roscpp_initializer.roscpp_shutdown()
515-
except rospy.ROSInterruptException:
516-
return
517-
except KeyboardInterrupt:
518-
return
507+
input("============ Press `Enter` to remove the box from the planning scene ...")
508+
tutorial.remove_box()
519509

510+
print("============ Python tutorial demo complete!")
511+
except rospy.ROSInterruptException:
512+
return
513+
except KeyboardInterrupt:
514+
return
520515

521516
if __name__ == '__main__':
522-
main()
517+
main()
523518

524519
## BEGIN_TUTORIAL
525520
## .. _moveit_commander:

0 commit comments

Comments
 (0)