class CartesianController(object): LABEL = 'None' def __init__(self): # Timeout (to filter out inactivity) self._timeout = 0.5 # Initializing the arm interface for the manipulator in the current # namespace self._arm_interface = ArmInterface() # Last goal for the end-effector pose/position self._last_goal = self._arm_interface.get_config_in_ee_frame('home') # Retrieve the publish rate self._publish_rate = 25 if not rospy.has_param('~cartesian_controller/publish_rate'): self._publish_rate = rospy.get_param('~cartesian_controller/publish_rate') if self._publish_rate <= 0: raise rospy.ROSException('Invalid negative publish rate') self._t_step = 0.01 if rospy.has_param("~tstep"): self._t_step = rospy.get_param('~tstep') if self._t_step <= 0: raise rospy.ROSException('Invalid translational step') self._r_step = 0.01 if rospy.has_param("~rstep"): self._r_step = rospy.get_param('~rstep') if self._r_step <= 0: raise rospy.ROSException('Invalid rotational step') # Default mapping for XBox 360 controller self._axes = dict(x=4, y=3, z=1, roll=0, pitch=7, yaw=6) if rospy.has_param('~axes'): axes = rospy.get_param('~axes') if type(axes) != dict: raise rospy.ROSException('Axes structure must be a dict') for tag in self._axes: if tag not in axes: raise rospy.ROSException('Axes for %s missing' % tag) self._axes[tag] = axes[tag] # Default for the RB button of the XBox 360 controller self._deadman_button = 5 if rospy.has_param('~deadman_button'): self._deadman_button = int(rospy.get_param('~deadman_button')) # If these buttons are pressed, the arm will not move if rospy.has_param('~exclusion_buttons'): self._exclusion_buttons = rospy.get_param('~exclusion_buttons') if type(self._exclusion_buttons) in [float, int]: self._exclusion_buttons = [int(self._exclusion_buttons)] elif type(self._exclusion_buttons) == list: for n in self._exclusion_buttons: if type(n) not in [float, int]: raise rospy.ROSException('Exclusion buttons must be an' ' integer index to the joystick button') else: self._exclusion_buttons = list() # Default for the start button of the XBox 360 controller self._home_button = 7 if rospy.has_param('~home_button'): self._home_button = int(rospy.get_param('~home_button')) # Get the transformation between the robot's base link and the # vehicle's base link self.listener = tf.TransformListener() # Get latest transform available latest = rospy.Time(0) base = self._arm_interface.namespace + 'base_link' self.listener.waitForTransform(base, self._arm_interface.base_link, latest, latest + rospy.Duration(100)) [pos, quat] = self.listener.lookupTransform( base, self._arm_interface.base_link, latest) rot = PyKDL.Rotation.Quaternion(quat[0], quat[1], quat[2], quat[3]) # Store transformation from the arm's base link and base self._trans = PyKDL.Frame(rot, PyKDL.Vector(pos[0], pos[1], pos[2])) # Velocity reference self._command = None # Last controller update self._last_controller_update = rospy.get_time() rospy.set_param('~cartesian_controller/name', self.LABEL) self._joint_effort_pub = dict() for joint in self._arm_interface.joint_names: self._joint_effort_pub[joint] = rospy.Publisher( self._arm_interface.namespace + joint + '/controller/command', Float64, queue_size=1) self._last_joy_update = rospy.get_time() self._joy_sub = rospy.Subscriber('joy', Joy, self._joy_callback) def _update(self, event): pass def _run(self): rate = rospy.Rate(self._publish_rate) while not rospy.is_shutdown(): self._update() rate.sleep() def _joy_callback(self, joy): self._command = np.zeros(6) if not joy.buttons[self._deadman_button] and self._deadman_button != -1: return for n in self._exclusion_buttons: if joy.buttons[n] == 1: return if joy.buttons[self._home_button] == 1: self._last_goal = self._arm_interface.get_config_in_ee_frame('home') self._last_joy_update = rospy.get_time() return self._command[0] = joy.axes[self._axes['x']] * self._t_step self._command[1] = joy.axes[self._axes['y']] * self._t_step self._command[2] = joy.axes[self._axes['z']] * self._t_step self._command[3] = joy.axes[self._axes['roll']] * self._r_step self._command[4] = joy.axes[self._axes['pitch']] * self._r_step self._command[5] = joy.axes[self._axes['yaw']] * self._r_step self._last_joy_update = rospy.get_time() def publish_joint_efforts(self, tau): # Publish torques t = np.asarray(tau).squeeze() for i, name in enumerate(self._arm_interface.joint_names): torque = Float64() torque.data = t[i] self._joint_effort_pub[name].publish(torque) # Update the time stamp self._last_controller_update = rospy.get_time()
class CartesianController(object): LABEL = 'None' def __init__(self): # Timeout (to filter out inactivity) self._timeout = 0.5 # Initializing the arm interface for the manipulator in the current # namespace self._arm_interface = ArmInterface() # Last goal for the end-effector pose/position self._last_goal = self._arm_interface.get_config_in_ee_frame('home') # Retrieve the publish rate self._publish_rate = 25 if rospy.has_param('~publish_rate'): self._publish_rate = rospy.get_param('~publish_rate') if self._publish_rate <= 0: raise rospy.ROSException('Invalid negative publish rate') self._tau = 0.1 # Get the transformation between the robot's base link and the # vehicle's base link self.listener = tf.TransformListener() # Get latest transform available latest = rospy.Time(0) base = self._arm_interface.namespace + 'base_link' # self.listener.waitForTransform(base, self._arm_interface.base_link, # latest, latest + rospy.Duration(100)) self.listener.waitForTransform(base, self._arm_interface.base_link, latest, rospy.Duration(100)) [pos, quat] = self.listener.lookupTransform(base, self._arm_interface.base_link, latest) rot = PyKDL.Rotation.Quaternion(quat[0], quat[1], quat[2], quat[3]) # Store transformation from the arm's base link and base self._trans = PyKDL.Frame(rot, PyKDL.Vector(pos[0], pos[1], pos[2])) # Velocity reference self._command = None # Last controller update self._last_controller_update = rospy.get_time() # Last velocity reference update time stamp self._last_reference_update = rospy.get_time() rospy.set_param('~name', self.LABEL) self._joint_effort_pub = dict() for joint in self._arm_interface.joint_names: self._joint_effort_pub[joint] = rospy.Publisher( self._arm_interface.namespace + joint + '/controller/command', Float64, queue_size=1) # Input velocity command subscriber, remap this topic to set a custom # command topic self._vel_command_sub = rospy.Subscriber('cmd_vel', Twist, self._vel_command_callback) # Topic that will receive the flag if the home button was pressed. If # the flag is true, the manipulator's goal is set to the stow # configuration self._home_pressed_sub = rospy.Subscriber('home_pressed', Bool, self._home_button_pressed) # Topic publishes the current goal set as reference to the manipulator self._goal_pub = rospy.Publisher('reference', PoseStamped, queue_size=1) # Topic to publish a visual marker for visualization of the current # reference in RViz self._goal_marker_pub = rospy.Publisher('reference_marker', Marker, queue_size=1) def _update(self, event): raise NotImplementedError() def _run(self): rate = rospy.Rate(self._publish_rate) while not rospy.is_shutdown(): self._update() rate.sleep() def _filter_input(self, state, cmd, dt): alpha = np.exp(-1 * dt / self._tau) return state * alpha + (1.0 - alpha) * cmd def _get_goal(self): if self._command is None or rospy.get_time( ) - self._last_reference_update > 0.1: return self._last_goal next_goal = deepcopy(self._last_goal) next_goal.p += PyKDL.Vector(self._command[0], self._command[1], self._command[2]) q_step = trans.quaternion_from_euler(self._command[3], self._command[4], self._command[5]) q_last = trans.quaternion_from_euler(*self._last_goal.M.GetRPY()) q_next = trans.quaternion_multiply(q_last, q_step) next_goal.M = PyKDL.Rotation.Quaternion(*q_next) g_pos = [next_goal.p.x(), next_goal.p.y(), next_goal.p.z()] g_quat = next_goal.M.GetQuaternion() 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.' return self._last_goal def _home_button_pressed(self, msg): if msg.data: self._command = np.zeros(6) self._last_goal = self._arm_interface.get_config_in_ee_frame( 'home') def _vel_command_callback(self, msg): dt = rospy.get_time() - self._last_reference_update if dt > 0.1: self._command = np.zeros(6) self._last_reference_update = rospy.get_time() return if self._command is None: self._command = np.zeros(6) self._command[0] = self._filter_input(self._command[0], msg.linear.x, dt) * dt self._command[1] = self._filter_input(self._command[1], msg.linear.y, dt) * dt self._command[2] = self._filter_input(self._command[2], msg.linear.z, dt) * dt self._command[3] = self._filter_input(self._command[3], msg.angular.x, dt) * dt self._command[4] = self._filter_input(self._command[4], msg.angular.y, dt) * dt self._command[5] = self._filter_input(self._command[5], msg.angular.z, dt) * dt self._last_reference_update = rospy.get_time() def publish_goal(self): # Publish the reference topic msg = PoseStamped() msg.header.frame_id = self._arm_interface.base_link msg.header.stamp = rospy.Time.now() msg.pose.position.x = self._last_goal.p.x() msg.pose.position.y = self._last_goal.p.y() msg.pose.position.z = self._last_goal.p.z() msg.pose.orientation = Quaternion(*self._last_goal.M.GetQuaternion()) self._goal_pub.publish(msg) marker = Marker() marker.header.frame_id = self._arm_interface.base_link marker.header.stamp = rospy.Time.now() marker.ns = self._arm_interface.arm_name marker.id = 0 marker.type = Marker.SPHERE marker.action = Marker.MODIFY marker.pose.position = Vector3(self._last_goal.p.x(), self._last_goal.p.y(), self._last_goal.p.z()) marker.pose.orientation = Quaternion( *self._last_goal.M.GetQuaternion()) marker.scale = Vector3(0.2, 0.2, 0.2) marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 self._goal_marker_pub.publish(marker) def publish_joint_efforts(self, tau): # Publish torques t = np.asarray(tau).squeeze() for i, name in enumerate(self._arm_interface.joint_names): torque = Float64() torque.data = t[i] self._joint_effort_pub[name].publish(torque) # Update the time stamp self._last_controller_update = rospy.get_time()
class CartesianController(object): LABEL = 'None' def __init__(self): # Timeout (to filter out inactivity) self._timeout = 0.5 # Initializing the arm interface for the manipulator in the current # namespace self._arm_interface = ArmInterface() # Last goal for the end-effector pose/position self._last_goal = self._arm_interface.get_config_in_ee_frame('home') # Retrieve the publish rate self._publish_rate = 25 if rospy.has_param('~publish_rate'): self._publish_rate = rospy.get_param('~publish_rate') if self._publish_rate <= 0: raise rospy.ROSException('Invalid negative publish rate') self._tau = 0.1 # Get the transformation between the robot's base link and the # vehicle's base link self.listener = tf.TransformListener() # Get latest transform available latest = rospy.Time(0) base = self._arm_interface.namespace + 'base_link' self.listener.waitForTransform(base, self._arm_interface.base_link, latest, latest + rospy.Duration(100)) [pos, quat] = self.listener.lookupTransform( base, self._arm_interface.base_link, latest) rot = PyKDL.Rotation.Quaternion(quat[0], quat[1], quat[2], quat[3]) # Store transformation from the arm's base link and base self._trans = PyKDL.Frame(rot, PyKDL.Vector(pos[0], pos[1], pos[2])) # Velocity reference self._command = None # Last controller update self._last_controller_update = rospy.get_time() # Last velocity reference update time stamp self._last_reference_update = rospy.get_time() rospy.set_param('~name', self.LABEL) self._joint_effort_pub = dict() for joint in self._arm_interface.joint_names: self._joint_effort_pub[joint] = rospy.Publisher( self._arm_interface.namespace + joint + '/controller/command', Float64, queue_size=1) # Input velocity command subscriber, remap this topic to set a custom # command topic self._vel_command_sub = rospy.Subscriber( 'cmd_vel', Twist, self._vel_command_callback) # Topic that will receive the flag if the home button was pressed. If # the flag is true, the manipulator's goal is set to the stow # configuration self._home_pressed_sub = rospy.Subscriber( 'home_pressed', Bool, self._home_button_pressed) # Topic publishes the current goal set as reference to the manipulator self._goal_pub = rospy.Publisher( 'reference', PoseStamped, queue_size=1) # Topic to publish a visual marker for visualization of the current # reference in RViz self._goal_marker_pub = rospy.Publisher( 'reference_marker', Marker, queue_size=1) def _update(self, event): raise NotImplementedError() def _run(self): rate = rospy.Rate(self._publish_rate) while not rospy.is_shutdown(): self._update() rate.sleep() def _filter_input(self, state, cmd, dt): alpha = np.exp(- 1 * dt / self._tau) return state * alpha + (1.0 - alpha) * cmd def _get_goal(self): if self._command is None or rospy.get_time() - self._last_reference_update > 0.1: return self._last_goal next_goal = deepcopy(self._last_goal) next_goal.p += PyKDL.Vector(self._command[0], self._command[1], self._command[2]) q_step = trans.quaternion_from_euler(self._command[3], self._command[4], self._command[5]) q_last = trans.quaternion_from_euler(*self._last_goal.M.GetRPY()) q_next = trans.quaternion_multiply(q_last, q_step) next_goal.M = PyKDL.Rotation.Quaternion(*q_next) g_pos = [next_goal.p.x(), next_goal.p.y(), next_goal.p.z()] g_quat = next_goal.M.GetQuaternion() 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.' return self._last_goal def _home_button_pressed(self, msg): if msg.data: self._command = np.zeros(6) self._last_goal = self._arm_interface.get_config_in_ee_frame('home') def _vel_command_callback(self, msg): dt = rospy.get_time() - self._last_reference_update if dt > 0.1: self._command = np.zeros(6) self._last_reference_update = rospy.get_time() return if self._command is None: self._command = np.zeros(6) self._command[0] = self._filter_input(self._command[0], msg.linear.x, dt) * dt self._command[1] = self._filter_input(self._command[1], msg.linear.y, dt) * dt self._command[2] = self._filter_input(self._command[2], msg.linear.z, dt) * dt self._command[3] = self._filter_input(self._command[3], msg.angular.x, dt) * dt self._command[4] = self._filter_input(self._command[4], msg.angular.y, dt) * dt self._command[5] = self._filter_input(self._command[5], msg.angular.z, dt) * dt self._last_reference_update = rospy.get_time() def publish_goal(self): # Publish the reference topic msg = PoseStamped() msg.header.frame_id = self._arm_interface.base_link msg.header.stamp = rospy.Time.now() msg.pose.position.x = self._last_goal.p.x() msg.pose.position.y = self._last_goal.p.y() msg.pose.position.z = self._last_goal.p.z() msg.pose.orientation = Quaternion(*self._last_goal.M.GetQuaternion()) self._goal_pub.publish(msg) marker = Marker() marker.header.frame_id = self._arm_interface.base_link marker.header.stamp = rospy.Time.now() marker.ns = self._arm_interface.arm_name marker.id = 0 marker.type = Marker.SPHERE marker.action = Marker.MODIFY marker.pose.position = Vector3(self._last_goal.p.x(), self._last_goal.p.y(), self._last_goal.p.z()) marker.pose.orientation = Quaternion(*self._last_goal.M.GetQuaternion()) marker.scale = Vector3(0.2, 0.2, 0.2) marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 self._goal_marker_pub.publish(marker) def publish_joint_efforts(self, tau): # Publish torques t = np.asarray(tau).squeeze() for i, name in enumerate(self._arm_interface.joint_names): torque = Float64() torque.data = t[i] self._joint_effort_pub[name].publish(torque) # Update the time stamp self._last_controller_update = rospy.get_time()