From b2a470980f916df14c62a719f2d89f3c44a73aec Mon Sep 17 00:00:00 2001 From: Laura Lindzey Date: Thu, 27 Aug 2020 01:58:35 -0700 Subject: [PATCH 1/5] Updating exception syntax to work for python 3; compatible with 2.6+ --- uuv_manipulators_control/scripts/gripper_controller.py | 2 +- uuv_manipulators_control/scripts/joint_position_controller.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/uuv_manipulators_control/scripts/gripper_controller.py b/uuv_manipulators_control/scripts/gripper_controller.py index 4b8c3d6..f9e845b 100755 --- a/uuv_manipulators_control/scripts/gripper_controller.py +++ b/uuv_manipulators_control/scripts/gripper_controller.py @@ -138,7 +138,7 @@ def _joy_callback(self, joy): self._ratio_goal = 0.0 if self._ratio_goal > 1: self._ratio_goal = 1.0 - except Exception, e: + except Exception as e: print 'Error occurred while parsing joystick input, check if the joy_id corresponds to the joystick ' \ 'being used. message=%s' % str(e) diff --git a/uuv_manipulators_control/scripts/joint_position_controller.py b/uuv_manipulators_control/scripts/joint_position_controller.py index ca38697..1a7239e 100755 --- a/uuv_manipulators_control/scripts/joint_position_controller.py +++ b/uuv_manipulators_control/scripts/joint_position_controller.py @@ -132,7 +132,7 @@ def _joy_callback(self, joy): # Check for the joint limits self._reference_pos[joint] = self._check_joint_limits(self._reference_pos[joint], joint) self._last_joy_update = rospy.get_time() - except Exception, e: + except Exception as e: print 'Error during joy parsing, message=', e if __name__ == '__main__': From 26eda3d1912cca2dfaf439e361a8ad55e066dce6 Mon Sep 17 00:00:00 2001 From: Laura Lindzey Date: Thu, 27 Aug 2020 02:19:16 -0700 Subject: [PATCH 2/5] Changing all print statements to use print_function --- .../scripts/gripper_controller.py | 6 ++++-- .../scripts/joint_position_controller.py | 3 ++- .../scripts/set_joint_config.py | 14 +++++++------- .../cartesian_controller.py | 4 ++-- .../src/uuv_manipulator_interfaces/arm.py | 3 ++- .../src/uuv_manipulator_interfaces/kin_chain.py | 16 ++++++++-------- 6 files changed, 25 insertions(+), 21 deletions(-) diff --git a/uuv_manipulators_control/scripts/gripper_controller.py b/uuv_manipulators_control/scripts/gripper_controller.py index f9e845b..4b2287c 100755 --- a/uuv_manipulators_control/scripts/gripper_controller.py +++ b/uuv_manipulators_control/scripts/gripper_controller.py @@ -13,6 +13,7 @@ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. +from __future__ import print_function import os import rospy from uuv_manipulator_interfaces import GripperInterface @@ -139,8 +140,9 @@ def _joy_callback(self, joy): if self._ratio_goal > 1: self._ratio_goal = 1.0 except Exception as e: - print 'Error occurred while parsing joystick input, check if the joy_id corresponds to the joystick ' \ - 'being used. message=%s' % str(e) + print('Error occurred while parsing joystick input, check if the ' + 'joy_id corresponds to the joystick being used. ' + 'message={}'.format(str(e))) if __name__ == '__main__': diff --git a/uuv_manipulators_control/scripts/joint_position_controller.py b/uuv_manipulators_control/scripts/joint_position_controller.py index 1a7239e..6de76c6 100755 --- a/uuv_manipulators_control/scripts/joint_position_controller.py +++ b/uuv_manipulators_control/scripts/joint_position_controller.py @@ -13,6 +13,7 @@ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. +from __future__ import print_function import os import rospy from copy import deepcopy @@ -133,7 +134,7 @@ def _joy_callback(self, joy): self._reference_pos[joint] = self._check_joint_limits(self._reference_pos[joint], joint) self._last_joy_update = rospy.get_time() except Exception as e: - print 'Error during joy parsing, message=', e + print('Error during joy parsing, message={}'.format(e)) if __name__ == '__main__': # Start the node diff --git a/uuv_manipulators_control/scripts/set_joint_config.py b/uuv_manipulators_control/scripts/set_joint_config.py index a597d2e..8f3c1dc 100755 --- a/uuv_manipulators_control/scripts/set_joint_config.py +++ b/uuv_manipulators_control/scripts/set_joint_config.py @@ -13,7 +13,7 @@ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. - +from __future__ import print_function import argparse import sys import rospy @@ -59,11 +59,11 @@ joint_pos = rospy.get_param(namespace + 'arms/' + arm_name + '/default_configs/' + config) - print 'Set joint configuration=', joint_pos - print 'Output topics:' + print('Set joint configuration={}'.format(joint_pos)) + print('Output topics:') pub = {} for joint in joint_pos: - print ' ', joint, '=', namespace + arm_name + '/' + joint + '/position_controller/command' + print(' {}={}/{}/position_controller/command'.format(joint, namespace + arm_name, joint)) pub[joint] = rospy.Publisher(namespace + arm_name + '/' + joint + '/position_controller/command', Float64, queue_size=1) start_time = time.clock() @@ -72,7 +72,7 @@ rospy.loginfo('Publishing command message for %s seconds' % duration) while time.clock() <= start_time + duration: if rospy.is_shutdown(): - print 'ROS is not running' + print('ROS is not running') break for joint in joint_pos: @@ -84,5 +84,5 @@ rospy.loginfo('Finishing setting joint configuration') except rospy.ROSInterruptException: - print 'uuv_manipulators_control::set_joint_config::Exception' - print 'Leaving uuv_manipulators_control::set_joint_config' + print('uuv_manipulators_control::set_joint_config::Exception') + print('Leaving uuv_manipulators_control::set_joint_config') diff --git a/uuv_manipulators_control/src/uuv_manipulators_control/cartesian_controller.py b/uuv_manipulators_control/src/uuv_manipulators_control/cartesian_controller.py index 87bacce..f1fbc93 100644 --- a/uuv_manipulators_control/src/uuv_manipulators_control/cartesian_controller.py +++ b/uuv_manipulators_control/src/uuv_manipulators_control/cartesian_controller.py @@ -12,7 +12,7 @@ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. - +from __future__ import print_function import rospy from uuv_manipulator_interfaces import ArmInterface from geometry_msgs.msg import Twist, PoseStamped, Quaternion, Vector3 @@ -139,7 +139,7 @@ def _get_goal(self): if self._arm_interface.inverse_kinematics(g_pos, g_quat) is not None: return next_goal else: - print 'Next goal could not be resolved by the inv. kinematics solver.' + print('Next goal could not be resolved by the inv. kinematics solver.') return self._last_goal def _home_button_pressed(self, msg): diff --git a/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/arm.py b/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/arm.py index 5f35dc1..fcf145a 100644 --- a/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/arm.py +++ b/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/arm.py @@ -12,6 +12,7 @@ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. +from __future__ import print_function from copy import deepcopy import rospy @@ -228,7 +229,7 @@ def get_config(self, config='home'): def add_callback(self, topic_name, function_handle): if topic_name not in self._subTopics: - print 'ArmInterface - Invalid topic name' + print('ArmInterface - Invalid topic name') return if topic_name not in self._callbacks: diff --git a/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/kin_chain.py b/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/kin_chain.py index 119973a..4646f9b 100644 --- a/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/kin_chain.py +++ b/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/kin_chain.py @@ -12,7 +12,7 @@ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. - +from __future__ import print_function import rospy from copy import deepcopy import numpy as np @@ -230,13 +230,13 @@ def print_robot_description(self): for j in self._robot_description.joints: if j.type != 'fixed': nf_joints += 1 - print 'Base root=%s' % self._base_link - print 'Tip link=%s' % self._tip_link - print "URDF non-fixed joints: %d;" % nf_joints - print "URDF total joints: %d" % len(self.n_joints) - print "URDF links: %d" % len(self._robot_description.links) - print "KDL joints: %d" % self._kdl_tree.getNrOfJoints() - print "KDL segments: %d" % self._kdl_tree.getNrOfSegments() + print('Base root=%s' % self._base_link) + print('Tip link=%s' % self._tip_link) + print("URDF non-fixed joints: %d;" % nf_joints) + print("URDF total joints: %d" % len(self.n_joints)) + print("URDF links: %d" % len(self._robot_description.links)) + print("KDL joints: %d" % self._kdl_tree.getNrOfJoints()) + print("KDL segments: %d" % self._kdl_tree.getNrOfSegments()) def print_chain(self): print 'Number of segments in chain=%d' % self._chain.getNrOfSegments() From abd2dfc5bc415be6481ee76f005ad6fa1a1c2cdd Mon Sep 17 00:00:00 2001 From: Laura Lindzey Date: Thu, 27 Aug 2020 02:39:28 -0700 Subject: [PATCH 3/5] Missed two print statements --- .../src/uuv_manipulator_interfaces/kin_chain.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/kin_chain.py b/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/kin_chain.py index 4646f9b..fe44bbd 100644 --- a/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/kin_chain.py +++ b/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/kin_chain.py @@ -239,9 +239,9 @@ def print_robot_description(self): print("KDL segments: %d" % self._kdl_tree.getNrOfSegments()) def print_chain(self): - print 'Number of segments in chain=%d' % self._chain.getNrOfSegments() + print('Number of segments in chain=%d' % self._chain.getNrOfSegments()) for idx in xrange(self._chain.getNrOfSegments()): - print '* ' + self._chain.getSegment(idx).getName() + print('* ' + self._chain.getSegment(idx).getName()) def get_joint_angle(self, joint): assert joint in self._joint_angles, 'Invalid joint name' From 3c1160ddfda98370118c5a1a96c56e722874541b Mon Sep 17 00:00:00 2001 From: Laura Lindzey Date: Thu, 27 Aug 2020 02:45:20 -0700 Subject: [PATCH 4/5] adding import for backwards-compatible xrange --- .../src/uuv_manipulator_interfaces/arm.py | 1 + .../src/uuv_manipulator_interfaces/kin_chain.py | 1 + 2 files changed, 2 insertions(+) diff --git a/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/arm.py b/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/arm.py index fcf145a..06eaad6 100644 --- a/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/arm.py +++ b/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/arm.py @@ -13,6 +13,7 @@ # See the License for the specific language governing permissions and # limitations under the License. from __future__ import print_function +from past.builtins import xrange from copy import deepcopy import rospy diff --git a/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/kin_chain.py b/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/kin_chain.py index fe44bbd..3987834 100644 --- a/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/kin_chain.py +++ b/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/kin_chain.py @@ -13,6 +13,7 @@ # See the License for the specific language governing permissions and # limitations under the License. from __future__ import print_function +from past.builtins import xrange import rospy from copy import deepcopy import numpy as np From 91021b3671668a6ff0ea26de52f69bf6fce304e3 Mon Sep 17 00:00:00 2001 From: Laura Lindzey Date: Thu, 27 Aug 2020 03:00:14 -0700 Subject: [PATCH 5/5] eff.values() -> listvalues(eff) --- .../src/uuv_manipulator_interfaces/arm.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/arm.py b/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/arm.py index 06eaad6..e41f330 100644 --- a/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/arm.py +++ b/uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/arm.py @@ -14,6 +14,7 @@ # limitations under the License. from __future__ import print_function from past.builtins import xrange +from future.utils import listvalues from copy import deepcopy import rospy @@ -170,7 +171,7 @@ def update_endeffector_state(self): # End effector pose pose = self.forward_position_kinematics(q) vel = self.forward_velocity_kinematics(q, qd) - wrench = np.dot(self.jacobian(q), np.array(eff.values())) + wrench = np.dot(self.jacobian(q), np.array(listvalues(eff))) # Store everything in the end point state message self._endeffector_state.position = pose[0:3] self._endeffector_state.orientation = pose[3::]