def callback(self, twist): msg = JogFrame() msg.header.stamp = rospy.Time.now() msg.header.frame_id = rospy.get_param('~frame_id', 'base_link') msg.group_name = rospy.get_param('~group_name', 'manipulator') msg.link_name = rospy.get_param('~link_name', 'tool0') if rospy.get_param('~dominant_mode', True): twist = self.dominantAxisMode(twist) if rospy.get_param('~axes_remap', True): twist = self.axesRemap(twist) msg.angular_delta.x = self.scale_angular['x'] * twist.angular.x msg.angular_delta.y = self.scale_angular['y'] * twist.angular.y msg.angular_delta.z = self.scale_angular['z'] * twist.angular.z msg.linear_delta.x = self.scale_linear['x'] * twist.linear.x msg.linear_delta.y = self.scale_linear['y'] * twist.linear.y msg.linear_delta.z = self.scale_linear['z'] * twist.linear.z msg.avoid_collisions = True self.pub.publish(msg)
def test_a_jog_with_angular_delta(self): '''Test to jog a command with angular delta''' # Get current posture of link_name (start_pos, start_rot) = self.listener.lookupTransform( self.frame_id, self.link_name, rospy.Time(0)) # Create jog frame command jog = JogFrame() jog.header.stamp = rospy.Time.now() jog.header.frame_id = self.frame_id jog.group_name = self.group_name jog.link_name = self.link_name jog.angular_delta.x = self.angular_delta / math.sqrt(3.0) jog.angular_delta.y = self.angular_delta / math.sqrt(3.0) jog.angular_delta.z = self.angular_delta / math.sqrt(3.0) self.pub.publish(jog) rospy.sleep(3.0) (pos, rot) = self.listener.lookupTransform( self.frame_id, self.link_name, rospy.Time(0)) # Check if the position is not changed for i in range(3): self.assertAlmostEqual(pos[i], start_pos[i], delta=0.0001) # Check if the rotaion is jogged by delta jog_q = tf.transformations.quaternion_about_axis(self.angular_delta, [1,1,1]) ans_rot = tf.transformations.quaternion_multiply(jog_q, start_rot) for i in range(4): self.assertAlmostEqual(rot[i], ans_rot[i], delta=0.0001)
def test_a_jog_with_linear_delta(self): '''Test to jog a command with linear delta''' # Get current posture of link_name (start_pos, start_rot) = self.listener.lookupTransform( self.frame_id, self.link_name, rospy.Time(0)) # Create jog frame command jog = JogFrame() jog.header.stamp = rospy.Time.now() jog.header.frame_id = self.frame_id jog.group_name = self.group_name jog.link_name = self.link_name jog.linear_delta.x = self.linear_delta jog.linear_delta.y = self.linear_delta jog.linear_delta.z = self.linear_delta self.pub.publish(jog) rospy.sleep(3.0) (pos, rot) = self.listener.lookupTransform( self.frame_id, self.link_name, rospy.Time(0)) # Check if the position is jogged by delta for i in range(3): self.assertAlmostEqual(pos[i], start_pos[i] + self.linear_delta, delta=0.0001) # Check if the rotaion is not changed rot_diff = tf.transformations.quaternion_multiply( tf.transformations.quaternion_inverse(start_rot), rot) for i in range(4): self.assertAlmostEqual(start_rot[i], rot[i], delta=0.0001)
def test_ten_jogs_with_linear_delta(self): '''Test to jog ten commands with linear delta''' # Get current posture of link_name (start_pos, start_rot) = self.listener.lookupTransform( self.frame_id, self.link_name, rospy.Time(0)) for i in range(10): # Create jog frame command jog = JogFrame() jog.header.stamp = rospy.Time.now() jog.header.frame_id = self.frame_id jog.group_name = self.group_name jog.link_name = self.link_name jog.linear_delta.x = self.linear_delta jog.linear_delta.y = self.linear_delta jog.linear_delta.z = self.linear_delta self.pub.publish(jog) rospy.sleep(3.0) (pos, rot) = self.listener.lookupTransform( self.frame_id, self.link_name, rospy.Time(0)) # Check if the position is jogged by delta for i in range(3): self.assertAlmostEqual(pos[i], start_pos[i] + self.linear_delta*10, delta=0.0001) # Check if the rotaion is not changed for i in range(4): self.assertAlmostEqual(start_rot[i], rot[i], delta=0.0001)
def callback(self, joy): if not joy.buttons[self.enable_button]: return msg = JogFrame() msg.header.stamp = rospy.Time.now() msg.header.frame_id = rospy.get_param('~frame_id', 'base_link') msg.group_name = rospy.get_param('~group_name', 'manipulator') msg.link_name = rospy.get_param('~link_name', 'tool0') if joy.buttons[self.angular_button]: msg.angular_delta.x = self.scale_angular['x'] * joy.axes[ self.axis_angular['x']] msg.angular_delta.y = self.scale_angular['y'] * joy.axes[ self.axis_angular['y']] msg.angular_delta.z = self.scale_angular['z'] * joy.axes[ self.axis_angular['z']] else: # These buttons are binary msg.linear_delta.x = self.scale_linear['x'] * joy.axes[ self.axis_linear['x']] msg.linear_delta.y = self.scale_linear['y'] * joy.axes[ self.axis_linear['y']] msg.linear_delta.z = self.scale_linear['z'] * joy.axes[ self.axis_linear['z']] msg.avoid_collisions = True self.pub.publish(msg)
def __init__(self, frame_id, group_name, link_name, linear_delta, angular_delta): # Get parameters self.frame_id = frame_id self.group_name = group_name self.link_name = link_name self.linear_delta = np.repeat(np.array([linear_delta]), 3) self.angular_delta = np.repeat(np.array([angular_delta]), 3) self._linear_flag = np.zeros(3) self._angular_flag = np.zeros(3) # Create jog frame command self._jog = JogFrame() self._jog.header.frame_id = self.frame_id self._jog.group_name = self.group_name self._jog.link_name = self.link_name # Publishers self._pub = rospy.Publisher('jog_frame', JogFrame, queue_size=10) self.r = rospy.Rate(2)