diff --git a/src/baxter_interface/robot_enable.py b/src/baxter_interface/robot_enable.py index 78bf45f..c12ef50 100644 --- a/src/baxter_interface/robot_enable.py +++ b/src/baxter_interface/robot_enable.py @@ -164,7 +164,7 @@ def reset(self): timeout_msg=error_env, body=pub.publish ) - except OSError, e: + except OSError as e: if e.errno == errno.ETIMEDOUT: if self._state.error == True and self._state.stopped == False: rospy.logwarn(error_nonfatal) diff --git a/src/joint_trajectory_action/joint_trajectory_action.py b/src/joint_trajectory_action/joint_trajectory_action.py index 0b0ec42..862f02e 100644 --- a/src/joint_trajectory_action/joint_trajectory_action.py +++ b/src/joint_trajectory_action/joint_trajectory_action.py @@ -28,14 +28,16 @@ """ Baxter RSDK Joint Trajectory Action Server """ +from __future__ import absolute_import + import bisect from copy import deepcopy import math import operator import numpy as np -import bezier -import minjerk +from . import bezier +from . import minjerk import rospy @@ -58,6 +60,11 @@ import baxter_interface +# Python2's xrange equals Python3's range, and xrange is removed on Python3 +if not hasattr(__builtins__, 'xrange'): + xrange = range + + class JointTrajectoryActionServer(object): def __init__(self, limb, reconfig_server, rate=100.0, mode='position_w_id', interpolation='bezier'): @@ -203,28 +210,29 @@ def _update_feedback(self, cmd_point, jnt_names, cur_time): self._fdbk.desired.time_from_start = rospy.Duration.from_sec(cur_time) self._fdbk.actual.positions = self._get_current_position(jnt_names) self._fdbk.actual.time_from_start = rospy.Duration.from_sec(cur_time) - self._fdbk.error.positions = map(operator.sub, - self._fdbk.desired.positions, - self._fdbk.actual.positions - ) + self._fdbk.error.positions = list(map(operator.sub, + self._fdbk.desired.positions, + self._fdbk.actual.positions + ) + ) self._fdbk.error.time_from_start = rospy.Duration.from_sec(cur_time) self._server.publish_feedback(self._fdbk) def _reorder_joints_ff_cmd(self, joint_names, point): - joint_name_order = self._limb.joint_names() - pnt = JointTrajectoryPoint() - pnt.time_from_start = point.time_from_start - pos_cmd = dict(zip(joint_names, point.positions)) - for jnt_name in joint_name_order: - pnt.positions.append(pos_cmd[jnt_name]) + joint_name_order = self._limb.joint_names() + pnt = JointTrajectoryPoint() + pnt.time_from_start = point.time_from_start + pos_cmd = dict(zip(joint_names, point.positions)) + for jnt_name in joint_name_order: + pnt.positions.append(pos_cmd[jnt_name]) if point.velocities: - vel_cmd = dict(zip(joint_names, point.velocities)) - for jnt_name in joint_name_order: - pnt.velocities.append(vel_cmd[jnt_name]) + vel_cmd = dict(zip(joint_names, point.velocities)) + for jnt_name in joint_name_order: + pnt.velocities.append(vel_cmd[jnt_name]) if point.accelerations: - accel_cmd = dict(zip(joint_names, point.accelerations)) - for jnt_name in joint_name_order: - pnt.accelerations.append(accel_cmd[jnt_name]) + accel_cmd = dict(zip(joint_names, point.accelerations)) + for jnt_name in joint_name_order: + pnt.accelerations.append(accel_cmd[jnt_name]) return pnt def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict):