def test_init_interface(self): arm = ArmInterface() # Test if the namespace and arm name are correct self.assertEquals(arm.namespace, '/rexrov/', 'Invalid robot namespace') self.assertEquals(arm.arm_name, 'oberon', 'Invalid arm name') self.assertEquals(arm.base_link, 'oberon/base', 'Invalid manipulator base name') self.assertEquals(arm.tip_link, 'oberon/end_effector', 'Invalid end-effector link name') self.assertNotEquals(len(arm.joint_names), 0, 'The list of joint names is empty') self.assertEquals(arm.n_links, 7, 'Invalid number of links') for name in arm.joint_names: self.assertIn( name, arm.joint_angles, 'Joint name %s not listed in the joint positions dictionary' % name) self.assertIn( name, arm.joint_velocities, 'Joint name %s not listed in the joint velocities dictionary' % name) self.assertIn( name, arm.joint_efforts, 'Joint name %s not listed in the joint efforts dictionary' % name)
def test_inv_kinematics(self): arm = ArmInterface() home_q = arm.home q = arm.inverse_kinematics(HOME_CART_POS, HOME_CART_ROT) for idx in range(len(arm.joint_names)): self.assertLess( np.abs(home_q[arm.joint_names[idx]] - q[idx]), 1e-6, 'home_q=%.4f, q=%.4f' % (home_q[arm.joint_names[idx]], q[idx]))
def test_joints_to_kdl(self): arm = ArmInterface() for idx, name in zip(range(len(arm.joint_names)), arm.joint_names): for t in ['positions', 'torques']: jnt_array = arm.joints_to_kdl(t, last_joint=name) self.assertEquals( jnt_array.rows(), idx + 1, 'Invalid number of joints, joint_idx=%d, last_joint=%s, n_joints=%d' % (idx, name, jnt_array.rows()))
def __init__(self): ns = [ item for item in rospy.get_namespace().split('/') if len(item) > 0 ] if len(ns) != 2: rospy.ROSException( 'The controller must be called in the manipulator namespace') self._namespace = ns[0] self._arm_name = ns[1] if self._namespace[0] != '/': self._namespace = '/' + self._namespace if self._namespace[-1] != '/': self._namespace += '/' # The arm interface loads the parameters from the URDF file and # parameter server and initializes the KDL tree self._arm_interface = ArmInterface() self._base_link = self._arm_interface.base_link self._tip_link = self._arm_interface.tip_link self._tip_frame = PyKDL.Frame() # KDL Solvers self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_interface.chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR( self._arm_interface.chain, self._arm_interface._fk_p_kdl, self._ik_v_kdl, 100, 1e-6) self._dyn_kdl = PyKDL.ChainDynParam(self._arm_interface.chain, PyKDL.Vector.Zero()) # Add a callback function to calculate the end effector's state by each # update of the joint state self._arm_interface.add_callback('joint_states', self.on_update_endeffector_state) # Publish the current manipulability index at each update of the joint # states # self._arm_interface.add_callback('joint_states', # self.publish_man_index) # Publish the end effector state self._endeffector_state_pub = rospy.Publisher('endpoint_state', EndPointState, queue_size=1) # Publish the manipulability index self._man_index_pub = rospy.Publisher('man_index', Float64, queue_size=1) self._services = dict() # Provide the service to calculate the inverse kinematics using the KDL solver self._services['ik'] = rospy.Service('ik_solver', SolveIK, self.solve_ik)
def test_jacobian(self): arm = ArmInterface() jac = arm.jacobian() self.assertIsNotNone(jac, 'Jacobian matrix is invalid') self.assertEquals(jac.shape, (arm.n_links, 6), 'The full Jacobian matrix has the wrong size') for idx, name in zip(range(len(arm.link_names)), arm.link_names): self.assertEquals( arm.jacobian(end_link=name).shape, (arm.n_links, 6)) self.assertEquals( arm.jacobian_transpose(end_link=name).shape, (6, arm.n_links))
def test_forward_position_kinematics(self): arm = ArmInterface() jnt_array = arm.home pose = arm.forward_position_kinematics(arm.home) self.assertLess( np.abs(pose[0:3] - HOME_CART_POS).sum(), 1e-6, 'Invalid output position, pos=[%.8f, %.8f, %.8f]' % (pose[0], pose[1], pose[2])) self.assertLess( np.abs(pose[3::] - HOME_CART_ROT).sum(), 1e-6, 'Invalid output orientation, rot=[%.8f, %.8f, %.8f, %.8f]' % (pose[3], pose[4], pose[5], pose[6]))
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 __init__(self): # Initializing arm interface self._arm_interface = ArmInterface() # PID controllers self._controllers = dict() # Current reference for each joint self._reference_pos = dict() # Output command topics self._command_topics = dict() # Axes mapping self._axes = dict() # Axes gain values self._axes_gain = dict() # 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')) # Last joystick update timestamp self._last_joy_update = rospy.get_time() # Joystick topic subscriber self._joy_sub = rospy.Subscriber('joy', Joy, self._joy_callback) # Reading the controller configuration controller_config = rospy.get_param('~controller_config') for joint in self._arm_interface.joint_names: for tag in controller_config: if tag in joint: try: # Read the controller parameters self._controllers[joint] = PIDRegulator( controller_config[tag]['controller']['p'], controller_config[tag]['controller']['i'], controller_config[tag]['controller']['d'], 1000) self._command_topics[joint] = rospy.Publisher( controller_config[tag]['topic'], Float64, queue_size=1) self._axes[joint] = controller_config[tag][ 'joint_input_axis'] self._axes_gain[joint] = controller_config[tag][ 'axis_gain'] # Setting the starting reference to the home position # in the robot parameters file self._reference_pos[joint] = deepcopy( self._arm_interface.home[joint]) except: raise rospy.ROSException( 'Error while trying to setup controller for joint <%s>' % joint) rate = rospy.Rate(50) while not rospy.is_shutdown(): pos = self._arm_interface.joint_angles for joint in pos: u = self._controllers[joint].regulate( self._reference_pos[joint] - pos[joint], rospy.get_time()) self._command_topics[joint].publish(Float64(u)) rate.sleep()
def test_home_config(self): arm = ArmInterface() self.assertIsNotNone(arm.home, 'Home configuration is invalid')
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() # 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') # Check if orientation will be controlled self._position_only = False if rospy.has_param('~position_only'): self._position_only = bool(rospy.get_param('~position_only')) # Check if cylindrical coordinates are to be used self._is_cylindrical = False if rospy.has_param('~is_cylindrical'): self._is_cylindrical = bool(rospy.get_param('~is_cylindrical')) # 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 # Subscribe to the twist reference topic self._command_sub = rospy.Subscriber('cartesian_controller/command', TwistStamped, self._command_callback, queue_size=1) # Last controller update self._last_controller_update = rospy.get_time() # Last goal for the end-effector pose/position self._last_goal = None try: # Wait for the IK solver service rospy.wait_for_service('ik_solver', timeout=2) except rospy.ROSException, e: print 'Service not available'
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)