def __init__(self, limb): self._kuka = URDF.from_parameter_server(key='robot_description') self._kdl_tree = kdl_tree_from_urdf_model(self._kuka) self._base_link = self._kuka.get_root() # self._tip_link = limb + '_gripper' self._tip_link = 'tool0' self._tip_frame = PyKDL.Frame() self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) # Kuka Interface Limb Instances # self._limb_interface = baxter_interface.Limb(limb) # self._joint_names = self._limb_interface.joint_names() self._joint_names = [ 'joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5', 'joint_a6' ] self._num_jnts = len(self._joint_names) # KDL Solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain) self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl) self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain) self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain, PyKDL.Vector.Zero())
def __init__(self, robot, ee_link=None): rospack = rospkg.RosPack() pykdl_dir = rospack.get_path('ur_pykdl') TREE_PATH = pykdl_dir + '/urdf/' + robot + '.urdf' self._ur = URDF.from_xml_file(TREE_PATH) self._kdl_tree = kdl_tree_from_urdf_model(self._ur) self._base_link = BASE_LINK ee_link = EE_LINK if ee_link is None else ee_link self._tip_link = ee_link self._tip_frame = PyKDL.Frame() self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) # UR Interface Limb Instances self._joint_names = JOINT_ORDER self._num_jnts = len(self._joint_names) # KDL Solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain) self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl) self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain) self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain, PyKDL.Vector.Zero())
def __init__(self, limb): self._pr2 = URDF.from_parameter_server(key='robot_description') self._kdl_tree = kdl_tree_from_urdf_model(self._pr2) self._base_link = self._pr2.get_root() # self._tip_link = limb + '_gripper' self._tip_link = limb + '_wrist_roll_link' self._tip_frame = PyKDL.Frame() self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) # Baxter Interface Limb Instances # self._limb_interface = baxter_interface.Limb(limb) # self._joint_names = self._limb_interface.joint_names() self._joint_names = [ limb + '_shoulder_pan_joint', limb + '_shoulder_lift_joint', limb + '_upper_arm_roll_joint', limb + '_elbow_flex_joint', limb + '_forearm_roll_joint', limb + '_wrist_flex_joint', limb + '_wrist_roll_joint' ] self._num_jnts = len(self._joint_names) # KDL Solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain) self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl) self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain) self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain, PyKDL.Vector.Zero())
def __init__(self, base_link=None, end_link=None): self._robot = kdl_parser_py.urdf.urdf.URDF.from_parameter_server( 'robot_description') (ok, self._kdl_tree) = kdl_parser_py.urdf.treeFromUrdfModel(self._robot) self._base_link = self._robot.get_root( ) if base_link is None else base_link self._tip_link = end_link self._tip_frame = PyKDL.Frame() self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) #self.joint_names = [joint.name for joint in self._robot.joints if joint.type!='fixed'] self.joint_names = [ self._arm_chain.getSegment(i).getJoint().getName() for i in range(self._arm_chain.getNrOfSegments()) if self._arm_chain.getSegment(i).getJoint().getType() != PyKDL.Joint.None ] self._num_jnts = len(self.joint_names) # Store joint information for future use self.get_joint_information() # KDL Solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain) self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain) #self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) #self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl) self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain) self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain, PyKDL.Vector.Zero())
def _init_real_consts(self): """ Initialize constants. """ self._home_position = self.cfgs.ARM.HOME_POSITION robot_description = self.cfgs.ROBOT_DESCRIPTION urdf_string = rospy.get_param(robot_description) self._num_ik_solver = trac_ik.IK(self.cfgs.ARM.ROBOT_BASE_FRAME, self.cfgs.ARM.ROBOT_EE_FRAME, urdf_string=urdf_string) _, urdf_tree = treeFromParam(robot_description) base_frame = self.cfgs.ARM.ROBOT_BASE_FRAME ee_frame = self.cfgs.ARM.ROBOT_EE_FRAME self._urdf_chain = urdf_tree.getChain(base_frame, ee_frame) self.arm_jnt_names = self._get_kdl_joint_names() self.arm_link_names = self._get_kdl_link_names() self.arm_dof = len(self.arm_jnt_names) self._jac_solver = kdl.ChainJntToJacSolver(self._urdf_chain) self._fk_solver_pos = kdl.ChainFkSolverPos_recursive(self._urdf_chain) self._fk_solver_vel = kdl.ChainFkSolverVel_recursive(self._urdf_chain) self.ee_link = self.cfgs.ARM.ROBOT_EE_FRAME
def __init__(self, limb='right'): self._sawyer = URDF.from_parameter_server(key='robot_description') self._kdl_tree = kdl_tree_from_urdf_model(self._sawyer) self._base_link = self._sawyer.get_root() # RUDI: LETS GET RID OF INTERA #self._tip_link = limb + '_hand' self._tip_link = 'right_gripper_tip' self._tip_frame = PyKDL.Frame() self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) # RUDI: LETS GET RID OF INTERA # Sawyer Interface Limb Instances # self._limb_interface = intera_interface.Limb(limb) #self._joint_names = self._limb_interface.joint_names() self._joint_names = ["right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5", "right_j6"] self._num_jnts = len(self._joint_names) # KDL Solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain) self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl) self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain) self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain, PyKDL.Vector.Zero())
def __init__(self, limb): self._sawyer = URDF.from_parameter_server(key='robot_description') self._kdl_tree = kdl_tree_from_urdf_model(self._sawyer) self._base_link = self._sawyer.get_root() print "BASE LINK:", self._base_link self._tip_link = limb + '_hand' self._tip_frame = PyKDL.Frame() self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) # Sawyer Interface Limb Instances self._limb_interface = intera_interface.Limb(limb) self._joint_names = self._limb_interface.joint_names() self._num_jnts = len(self._joint_names) # KDL Solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain) self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl) self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain) self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain, PyKDL.Vector(0.0, 0.0, -9.81))
def __init__(self, urdf_param='robot_description'): self._urdf = URDF.from_parameter_server(urdf_param) (parse_ok, self._kdl_tree) = treeFromUrdfModel(self._urdf) # Check @TODO Exception if not parse_ok: rospy.logerr( 'Error parsing URDF from parameter server ({0})'.format( urdf_param)) else: rospy.logdebug('Parsing URDF succesful') self._base_link = self._urdf.get_root() # @TODO Hardcoded self._tip_link = 'link_6' self._tip_frame = PyKDL.Frame() self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) # @TODO Hardcoded self._joint_names = ['a1', 'a2', 'a3', 'a4', 'a5', 'a6'] self._num_joints = len(self._joint_names) # KDL Solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain) self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl) self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain) self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain, PyKDL.Vector.Zero())
def __init__(self, limb, description=None): if description is None: self._franka = URDF.from_parameter_server(key='robot_description') else: self._franka = URDF.from_xml_file(description) self._kdl_tree = kdl_tree_from_urdf_model(self._franka) self._base_link = self._franka.get_root() self._tip_link = limb.name + ('_hand' if limb.has_gripper else '_link8') self._tip_frame = PyKDL.Frame() self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) self._limb_interface = limb self._joint_names = deepcopy(self._limb_interface.joint_names()) self._num_jnts = len(self._joint_names) # KDL Solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain) self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl) self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain) self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain, PyKDL.Vector.Zero())
def __init__(self, configs, moveit_planner='ESTkConfigDefault', analytical_ik=None, use_moveit=True): """ Constructor for Arm parent class. :param configs: configurations for arm :param moveit_planner: moveit planner type :param analytical_ik: customized analytical ik class if you have one, None otherwise :param use_moveit: use moveit or not, default is True :type configs: YACS CfgNode :type moveit_planner: string :type analytical_ik: None or an Analytical ik class :type use_moveit: bool """ self.configs = configs self.moveit_planner = moveit_planner self.moveit_group = None self.scene = None self.use_moveit = use_moveit self.joint_state_lock = threading.RLock() self.tf_listener = tf.TransformListener() if self.use_moveit: self._init_moveit() robot_description = self.configs.ARM.ARM_ROBOT_DSP_PARAM_NAME urdf_string = rospy.get_param(robot_description) self.num_ik_solver = trac_ik.IK(configs.ARM.ARM_BASE_FRAME, configs.ARM.EE_FRAME, urdf_string=urdf_string) if analytical_ik is not None: self.ana_ik_solver = analytical_ik(configs.ARM.ARM_BASE_FRAME, configs.ARM.EE_FRAME) _, self.urdf_tree = treeFromParam(robot_description) self.urdf_chain = self.urdf_tree.getChain(configs.ARM.ARM_BASE_FRAME, configs.ARM.EE_FRAME) self.arm_joint_names = self._get_kdl_joint_names() self.arm_link_names = self._get_kdl_link_names() self.arm_dof = len(self.arm_joint_names) self.jac_solver = kdl.ChainJntToJacSolver(self.urdf_chain) self.fk_solver_pos = kdl.ChainFkSolverPos_recursive(self.urdf_chain) self.fk_solver_vel = kdl.ChainFkSolverVel_recursive(self.urdf_chain) # Subscribers self._joint_angles = dict() self._joint_velocities = dict() self._joint_efforts = dict() rospy.Subscriber(configs.ARM.ROSTOPIC_JOINT_STATES, JointState, self._callback_joint_states) # Publishers self.joint_pub = None self._setup_joint_pub()
def __init__(self): # Read parameters self.frame_id = read_parameter('~frame_id', 'base_link') self.tip_link = read_parameter('~tip_link', 'end_effector') # Kinematics self.urdf = URDF.from_parameter_server(key='robot_description') self.kinematics = KDLKinematics(self.urdf, self.frame_id, self.tip_link) self.fk_vel_solver = PyKDL.ChainFkSolverVel_recursive( self.kinematics.chain) # Set-up publishers/subscribers self.state_pub = rospy.Publisher('/grips/endpoint_state', EndpointState) rospy.Subscriber('/joint_states', JointState, self.joint_states_cb)
def initialize_kinematic_solvers(self): robot_description = rospy.get_param('/yumi/velocity_control/robot_description', '/robot_description') self._robot = URDF.from_parameter_server(key=robot_description) self._kdl_tree = kdl_tree_from_urdf_model(self._robot) # self._base_link = self._robot.get_root() self._ee_link = 'gripper_' +self._arm_name + '_base' #name of the frame we want self._ee_frame = PyKDL.Frame() self._ee_arm_chain = self._kdl_tree.getChain(self._base_link, self._ee_link) # KDL Solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._ee_arm_chain) self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._ee_arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._ee_arm_chain) #self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl) self._ik_p_kdl = PyKDL.ChainIkSolverPos_LMA(self._ee_arm_chain) self._jac_kdl = PyKDL.ChainJntToJacSolver(self._ee_arm_chain) self._dyn_kdl = PyKDL.ChainDynParam(self._ee_arm_chain, PyKDL.Vector.Zero())
class MoveGroupLeftArm(object): def __init__(self): #----------------control init begin----------------------------------------------- super(MoveGroupLeftArm, self).__init__() moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('iiwa_move_to_ee_pose', anonymous=True) self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() group_name = "manipulator" self.group = moveit_commander.MoveGroupCommander(group_name) #group.set_max_velocity_scaling_factor(0.15) self.group.set_max_acceleration_scaling_factor(0.1) if self.group.set_planner_id(PLANNER_ID): print "Using planner: %s" % PLANNER_ID self.group.set_num_planning_attempts(100) self.group.set_planning_time(3) self.group.set_start_state_to_current_state() self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.gripper_io_publisher = rospy.Publisher('command/GripperState', iiwa_msgs.msg.GripperState, queue_size=10) self.upright_constraints = Constraints() try: rospy.wait_for_service('configuration/setSmartServoLimits', 1) except: print 'Service call timeout' try: self.set_speed_limits = rospy.ServiceProxy('configuration/setSmartServoLimits', SetSmartServoJointSpeedLimits) response = self.set_speed_limits(0.3, 0.1, -1) print 'Velocity limit set' print response except rospy.ServiceException, e: print "service call failed: %s"%e #----------------kinematics init begin--------------------------------------------- _iiwa_URDF = URDF.from_parameter_server(key='robot_description') _iiwa_kdl_tree = kdl_tree_from_urdf_model(_iiwa_URDF) _iiwa_base_link = _iiwa_URDF.get_root() self.iiwa_chain = _iiwa_kdl_tree.getChain(_iiwa_base_link, 'tool_link_ee') self.forward_kin_position_kdl = PyKDL.ChainFkSolverPos_recursive(self.iiwa_chain) _forward_kin_velocity_kdl = PyKDL.ChainFkSolverVel_recursive(self.iiwa_chain) self.inverse_kin_velocity_kdl = PyKDL.ChainIkSolverVel_pinv(self.iiwa_chain) self.min_limits = PyKDL.JntArray(NUM_JOINTS) self.max_limits = PyKDL.JntArray(NUM_JOINTS) for idx, jnt in enumerate(MIN_JOINT_LIMITS_DEG): self.min_limits[idx] = math.radians(jnt) for idx, jnt in enumerate(MAX_JOINT_LIMITS_DEG): self.max_limits[idx] = math.radians(jnt) self.component_map = {}
def __init__(self, limb, ee_frame_name="panda_link8", additional_segment_config=None, description=None): if description is None: self._franka = URDF.from_parameter_server(key='robot_description') else: self._franka = URDF.from_xml_file(description) self._kdl_tree = kdl_tree_from_urdf_model(self._franka) if additional_segment_config is not None: for c in additional_segment_config: q = quaternion.from_rotation_matrix(c["origin_ori"]).tolist() kdl_origin_frame = PyKDL.Frame( PyKDL.Rotation.Quaternion(q.x, q.y, q.z, q.w), PyKDL.Vector(*(c["origin_pos"].tolist()))) kdl_sgm = PyKDL.Segment(c["child_name"], PyKDL.Joint(c["joint_name"]), kdl_origin_frame, PyKDL.RigidBodyInertia()) self._kdl_tree.addSegment(kdl_sgm, c["parent_name"]) self._base_link = self._franka.get_root() self._tip_link = ee_frame_name self._tip_frame = PyKDL.Frame() self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) self._limb_interface = limb self._joint_names = deepcopy(self._limb_interface.joint_names()) self._num_jnts = len(self._joint_names) # KDL Solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain) self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl) self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain) self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain, PyKDL.Vector.Zero())
def create_chain(self, ee_frame_name): self._tip_link = ee_frame_name self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) self._joint_names = deepcopy(self._limb_interface.joint_names()) self._num_jnts = len(self._joint_names) # KDL Solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain) self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl) self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain) self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain, PyKDL.Vector.Zero())
def _init_kinematics(self): # Ros-Params base_link = rospy.get_param("pyrobot/base_link") gripper_link = rospy.get_param("pyrobot/gripper_link") robot_description = rospy.get_param("pyrobot/robot_description") urdf_string = rospy.get_param(robot_description) self.num_ik_solver = trac_ik.IK(base_link, gripper_link, urdf_string=urdf_string) _, self.urdf_tree = treeFromParam(robot_description) self.urdf_chain = self.urdf_tree.getChain(base_link, gripper_link) self.arm_joint_names = self._get_kdl_joint_names() self.arm_link_names = self._get_kdl_link_names() self.arm_dof = len(self.arm_joint_names) self.jac_solver = kdl.ChainJntToJacSolver(self.urdf_chain) self.fk_solver_pos = kdl.ChainFkSolverPos_recursive(self.urdf_chain) self.fk_solver_vel = kdl.ChainFkSolverVel_recursive(self.urdf_chain)
def __init__(self): limb = read_parameter('~limb', 'right') if limb not in ['right', 'left']: rospy.logerr('Unknown limb name [%s]' % limb) return # Read the controllers parameters gains = read_parameter('~gains', dict()) self.kp = joint_list_to_kdl(gains['Kp']) self.kd = joint_list_to_kdl(gains['Kd']) self.publish_rate = read_parameter('~publish_rate', 500) self.frame_id = read_parameter('~frame_id', 'base') self.timeout = read_parameter('~timeout', 0.02) # seconds # Set-up baxter interface self.arm_interface = baxter_interface.Limb(limb) # set_joint_torques must be commanded at a rate great than the timeout specified by set_command_timeout. # If the timeout is exceeded before a new set_joint_velocities command is received, the controller will switch # modes back to position mode for safety purposes. self.arm_interface.set_command_timeout(self.timeout) # Baxter kinematics self.urdf = URDF.from_parameter_server(key='robot_description') self.tip_link = '%s_gripper' % limb self.kinematics = KDLKinematics(self.urdf, self.frame_id, self.tip_link) self.fk_vel_solver = PyKDL.ChainFkSolverVel_recursive( self.kinematics.chain) # Adjust the publish rate of baxter's state joint_state_pub_rate = rospy.Publisher( '/robot/joint_state_publish_rate', UInt16) # Get the joint names and limits self.joint_names = self.arm_interface.joint_names() self.num_joints = len(self.joint_names) self.lower_limits = np.zeros(self.num_joints) self.upper_limits = np.zeros(self.num_joints) # KDL joint may be in a different order than expected kdl_lower_limits, kdl_upper_limits = self.kinematics.get_joint_limits() for i, name in enumerate(self.kinematics.get_joint_names()): if name in self.joint_names: idx = self.joint_names.index(name) self.lower_limits[idx] = kdl_lower_limits[i] self.upper_limits[idx] = kdl_upper_limits[i] self.middle_values = (self.upper_limits + self.lower_limits) / 2.0 # Move the arm to a neutral position #~ self.arm_interface.move_to_neutral(timeout=10.0) #~ neutral_pos = [-math.pi/4, 0.0, 0.0, 0.0, 0.0, math.pi/2, -math.pi/2] neutral_pos = [1.0, -0.57, -1.2, 1.3, 0.86, 1.4, -0.776] #~ neutral_pos = [pi/4, -pi/3, 0.0, pi/2, 0.0, pi/3, 0.0] neutral_cmd = dict() for i, name in enumerate(self.joint_names): if name in ['left_s0', 'left_w2']: neutral_cmd[name] = neutral_pos[i] * -1.0 else: neutral_cmd[name] = neutral_pos[i] self.arm_interface.move_to_joint_positions(neutral_cmd, timeout=10.0) # Baxter faces self.limb = limb self.face = FaceImage() self.face.set_image('happy') # Set-up publishers/subscribers self.suppress_grav_comp = rospy.Publisher( '/robot/limb/%s/suppress_gravity_compensation' % limb, Empty) joint_state_pub_rate.publish(int(self.publish_rate)) self.feedback_pub = rospy.Publisher('/takktile/force_feedback', Wrench) rospy.Subscriber('/baxter/%s_ik_command' % limb, PoseStamped, self.ik_command_cb) self.gripper_closed = False rospy.Subscriber('/takktile/calibrated', Touch, self.takktile_cb) rospy.Subscriber('/robot/end_effector/%s_gripper/state' % limb, EndEffectorState, self.end_effector_cb) rospy.loginfo('Running Cartesian controller for the %s arm' % limb) # Start torque controller timer self.cart_command = None self.torque_controller_timer = rospy.Timer( rospy.Duration(1.0 / self.publish_rate), self.torque_controller_cb) # Shutdown hookup to clean-up before killing the script rospy.on_shutdown(self.shutdown)
def __init__(self, name, base, ee_link, namespace=None, arm_name=None, compute_fk_for_all=False): # Joint states self._joint_angles = dict() self._joint_velocity = dict() self._joint_effort = dict() # Finger chain name self._name = name # Namespace if None in [namespace, arm_name]: ns = [ item for item in rospy.get_namespace().split('/') if len(item) > 0 ] if len(ns) != 2: rospy.ROSException( 'The controller must be called inside the namespace the manipulator namespace' ) self._namespace = ns[0] self._arm_name = ns[1] else: self._namespace = namespace self._arm_name = arm_name if self._namespace[0] != '/': self._namespace = '/' + self._namespace if self._namespace[-1] != '/': self._namespace += '/' # True if it has to compute the forward kinematics for all frames self._compute_fk_for_all = compute_fk_for_all # List of frames for each link self._frames = dict() # Load robot description (complete robot, including vehicle, if # available) self._robot_description = URDF.from_parameter_server( key=self._namespace + 'robot_description') # KDL tree of the whole structure ok, self._kdl_tree = treeFromUrdfModel(self._robot_description) # Base link self._base_link = base # Tip link self._tip_link = ee_link # Read the complete link name, with robot's namespace, if existent for link in self._robot_description.links: if self._arm_name not in link.name: continue linkname = link.name.split('/')[-1] if self._base_link == linkname: self._base_link = link.name if self._tip_link == linkname: self._tip_link = link.name # Get arm chain self._chain = self._kdl_tree.getChain(self._base_link, self._tip_link) # Partial tree from base to each link self._chain_part = dict() for link in self._robot_description.links: if link.name in self.link_names: self._chain_part[link.name] = self._kdl_tree.getChain( self._base_link, link.name) # Get joint names from the chain # Joint names self._joint_names = list() for idx in xrange(self._chain.getNrOfSegments()): joint = self._chain.getSegment(idx).getJoint() # Not considering fixed joints if joint.getType() == 0: name = joint.getName() self._joint_names.append(name) self._joint_angles[name] = 0.0 self._joint_velocity[name] = 0.0 self._joint_effort[name] = 0.0 # Jacobian solvers self._jac_kdl = PyKDL.ChainJntToJacSolver(self._chain) # Jacobian solver dictionary for partial trees self._jac_kdl_part = dict() for link in self._robot_description.links: if link.name in self.link_names: self._jac_kdl_part[link.name] = PyKDL.ChainJntToJacSolver( self._chain_part[link.name]) # Forward position kinematics solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._chain) # Forward velocity kinematics solvers self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._chain) # Inverse kinematic solvers self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._chain, self._fk_p_kdl, self._ik_v_kdl, 100, 1e-6)