def prepare_kin_dyn(self): robot_description = rospy.get_param("/robot_description") flag, self.tree = kdl_parser.treeFromParam(robot_description) chain = self.tree.getChain(self.base_link, self.rl_link) self.fk_bl = kdl.ChainFkSolverPos_recursive(chain) self.jac_bl = kdl.ChainJntToJacSolver(chain) chain = self.tree.getChain(self.base_link, self.rr_link) self.fk_br = kdl.ChainFkSolverPos_recursive(chain) self.jac_br = kdl.ChainJntToJacSolver(chain) chain = self.tree.getChain(self.base_link, self.fl_link) self.fk_fl = kdl.ChainFkSolverPos_recursive(chain) self.jac_fl = kdl.ChainJntToJacSolver(chain) chain = self.tree.getChain(self.base_link, self.fr_link) self.fk_fr = kdl.ChainFkSolverPos_recursive(chain) self.jac_fr = kdl.ChainJntToJacSolver(chain) # convention front_left, front_front, back_right, back_left self.jac_list = [self.jac_fl, self.jac_fr, self.jac_bl, self.jac_br] self.fk_list = [self.fk_fl, self.fk_fr, self.fk_bl, self.fk_br] joints = kdl.JntArray(3) joints[0] = 0 joints[1] = 0 joints[2] = 0 frame = kdl.Frame() jk_list = self.fk_list
def __init__(self, env, use_qp_solver=False, check_joint_limits=False, use_mocap_ctrl=True, **kwargs): super(YumiBarAgent, self).__init__(env, **kwargs) import PyKDL from gym.utils.kdl_parser import kdl_tree_from_urdf_model from gym.envs.yumi.yumi_env import YumiEnv assert isinstance(env.unwrapped, YumiEnv) assert env.unwrapped.has_two_arms assert not env.unwrapped.block_gripper self._raw_env = env.unwrapped # type: YumiEnv self._sim = self._raw_env.sim self._dt = env.unwrapped.dt self._goal = None self._phase = 0 self._phase_steps = 0 self._locked_l_to_r_tf = None self._robot = YumiEnv.get_urdf_model() self._kdl = PyKDL self.use_mocap_ctrl = use_mocap_ctrl self.use_qp_solver = use_qp_solver self.check_joint_limits = check_joint_limits if check_joint_limits and not use_qp_solver: raise ValueError( 'Joint limits can only be checked with the QP solver!') # make sure the simulator is ready before getting the transforms self._sim.forward() self._base_offset = self._sim.data.get_body_xpos( 'yumi_base_link').copy() kdl_tree = kdl_tree_from_urdf_model(self._robot) base_link = self._robot.get_root() ee_arm_chain_r = kdl_tree.getChain(base_link, 'gripper_r_base') ee_arm_chain_l = kdl_tree.getChain(base_link, 'gripper_l_base') if self._raw_env.has_right_arm: self._add_site_to_chain(ee_arm_chain_r, 'gripper_r_center') if self._raw_env.has_left_arm: self._add_site_to_chain(ee_arm_chain_l, 'gripper_l_center') self._fkp_solver_r = PyKDL.ChainFkSolverPos_recursive(ee_arm_chain_r) self._fkp_solver_l = PyKDL.ChainFkSolverPos_recursive(ee_arm_chain_l) self._ikp_solver_r = PyKDL.ChainIkSolverPos_LMA(ee_arm_chain_r) self._ikp_solver_l = PyKDL.ChainIkSolverPos_LMA(ee_arm_chain_l) self._jac_solver_r = PyKDL.ChainJntToJacSolver(ee_arm_chain_r) self._jac_solver_l = PyKDL.ChainJntToJacSolver(ee_arm_chain_l)
def __init__(self, urdf, world='world', tip='tip'): # Try loading the URDF data into a KDL tree. (ok, self.tree) = kdlp.treeFromString(urdf) assert ok, "Unable to parse the URDF" # Save the base and tip frame names. self.baseframe = world self.tipframe = tip # Create the isolated chain from world to tip. self.chain = self.tree.getChain(world, tip) # Extract the number of joints. self.N = self.chain.getNrOfJoints() assert self.N > 0, "Found no joints in the chain" # Create storage for the joint position, tip position, and # Jacobian matrices (for the KDL library). self.qkdl = kdl.JntArray(self.N) self.Tkdl = kdl.Frame() self.Jkdl = kdl.Jacobian(self.N) # Also pre-allocate the memory for the numpy return variables. # The content will be overwritten so the initial values are # irrelevant. self.p = np.zeros((3, 1)) self.R = np.identity(3) self.J = np.zeros((6, self.N)) # Instantiate the solvers for tip position and Jacobian. self.fkinsolver = kdl.ChainFkSolverPos_recursive(self.chain) self.jacsolver = kdl.ChainJntToJacSolver(self.chain)
def __init__(self): self.dh_params = [[-0.033, pi / 2, 0.145, pi, -1], [0.155, 0, 0, pi / 2, -1], [0.135, 0, 0, 0.0, -1], [-0.002, pi / 2, 0, -pi / 2, -1], [0, pi, -0.185, -pi, -1]] self.joint_offset = [170*pi/180, 65*pi/180, -146*pi/180, 102.5*pi/180, 167.5*pi/180] self.joint_limit_min = [-169*pi/180, -65*pi/180, -151*pi/180, -102.5*pi/180, -167.5*pi/180] self.joint_limit_max = [169*pi/180, 90*pi/180, 146*pi/180, 102.5*pi/180, 167.5*pi/180] # Setup the subscribers for the joint states self.subscriber_joint_state_ = rospy.Subscriber('/joint_states', JointState, self.joint_state_callback, queue_size=5) # TF2 broadcaster self.pose_broadcaster = tf2_ros.TransformBroadcaster() # PyKDL self.kine_chain = PyKDL.Chain() self.setup_kdl_chain() self.current_joint_position = PyKDL.JntArray(self.kine_chain.getNrOfJoints()) self.current_pose = PyKDL.Frame() self.fk_solver = PyKDL.ChainFkSolverPos_recursive(self.kine_chain) self.ik_solver = PyKDL.ChainIkSolverPos_LMA(self.kine_chain) self.jac_calc = PyKDL.ChainJntToJacSolver(self.kine_chain)
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_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, urdf_relative_path='../../assets/urdf/sawyer_arm_intera.urdf'): ''' Uses inverse kinematics in order to go from end-effector cartesian velocity commands to joint velocity commands :param urdf_relative_path: The path to the URDF file of the robot, relative to the current directory ''' self.num_joints = 7 # Get the URDF urdf_path = os.path.join(os.path.dirname(os.path.realpath(__file__)), urdf_relative_path) self.urdf_model = URDF.from_xml_string(open(urdf_path).read()) # Set the base link, and the tip link. I think that these are just strings, defined somewhere in the URDF. The name 'right_hand' is how the gripper is described on the real Sawyer, but it may have a different name in your own URDF. self.base_link = self.urdf_model.get_root() self.tip_link = "right_hand" # # Create a KDL tree from the URDF model. This is an object defining the kinematics of the entire robot. self.kdl_tree = treeFromUrdfModel(self.urdf_model) # Create a KDL chain from the tree. This is one specific chain within the overall kinematics model. self.arm_chain = self.kdl_tree[1].getChain(self.base_link, self.tip_link) # Create a solver which will be used to compute the forward kinematics self.forward_kinematics_solver = PyKDL.ChainFkSolverPos_recursive( self.arm_chain) # Create a solver which will be used to compute the Jacobian self.jacobian_solver = PyKDL.ChainJntToJacSolver(self.arm_chain) #Velocity inverse kinematics solver self.IK_v_solver = PyKDL.ChainIkSolverVel_pinv(self.arm_chain) # Create a solver to retrieve the joint angles form the eef position self.IK_solver = PyKDL.ChainIkSolverPos_NR( self.arm_chain, self.forward_kinematics_solver, self.IK_v_solver)
def _setup(self): # set up chebychev points self.resolution = 100 self.duration = 0.8 # load in ros parameters self.baselink = rospy.get_param("blue_hardware/baselink") self.endlink = rospy.get_param("blue_hardware/endlink") urdf = rospy.get_param("/robot_description") flag, self.tree = kdl_parser.treeFromString(urdf) # build kinematic chain and fk and jacobian solvers chain_ee = self.tree.getChain(self.baselink, self.endlink) self.fk_ee = kdl.ChainFkSolverPos_recursive(chain_ee) self.jac_ee = kdl.ChainJntToJacSolver(chain_ee) # building robot joint state self.num_joints = chain_ee.getNrOfJoints() self.joint_names = rospy.get_param("blue_hardware/joint_names") self.joint_names = self.joint_names[:self.num_joints] if self.debug: rospy.loginfo(self.joint_names) self.joints = kdl.JntArray(self.num_joints) # todo make seperate in abstract class self.ik = TRAC_IK(self.baselink, self.endlink, urdf, 0.005, 5e-5, "Distance")
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__(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, 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, 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, hover_distance=0.15, verbose=True): self._baxter = URDF.from_parameter_server( key='robot_description' ) #Get Baxter URDF Model from ROS Parameter Server self._kdl_tree = kdl_tree_from_urdf_model( self._baxter) #Get kdl tree Baxter URDF Model self._limb_name = limb # string self._hover_distance = hover_distance # in meters self._verbose = verbose # bool self._limb = baxter_interface.Limb(limb) #Creates a Limb Object self._gripper = baxter_interface.Gripper( limb) #Creates a Gripper Object from the Limb Object #self._kin = baxter_kinematics(limb) #self._traj = Trajectory(self._limb_name) self._base_link = self._baxter.get_root( ) #Gets root link of the Baxter URDF model self._tip_link = limb + '_gripper' #Gets the gripper link of ther respective limb self._arm_chain = self._kdl_tree.getChain( self._base_link, self._tip_link) #Creates chain from root link and gripper link self._jac_kdl = PyKDL.ChainJntToJacSolver( self._arm_chain ) #Creates a jacobian solver for the Baxter from the chain self._joint_names = self._limb.joint_names( ) #List of joint names of the respective limb self._trajectory = list() #List of Nominal Trajectory Waypoints ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK) rospy.wait_for_service(ns, 5.0) # verify robot is enabled print("Getting robot state... ") self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable()
def __init__(self): self.num_joints = 7 # Get the URDF urdf_path = os.path.join(os.path.dirname(os.path.realpath(__file__)), '../assets/urdf/sawyer_intera.urdf') self.urdf_model = URDF.from_xml_string(open(urdf_path).read()) # Set the base link, and the tip link. self.base_link = self.urdf_model.get_root() self.tip_link = "right_hand" # # Create a KDL tree from the URDF model. This is an object defining the kinematics of the entire robot. self.kdl_tree = treeFromUrdfModel(self.urdf_model) # Create a KDL chain from the tree. This is one specific chain within the overall kinematics model. self.arm_chain = self.kdl_tree[1].getChain(self.base_link, self.tip_link) # Create a solver which will be used to compute the forward kinematics self.forward_kinematics_solver = PyKDL.ChainFkSolverPos_recursive( self.arm_chain) # Create a solver which will be used to compute the Jacobian self.jacobian_solver = PyKDL.ChainJntToJacSolver(self.arm_chain) #Velocity inverse kinematics solver self.IK_v_solver = PyKDL.ChainIkSolverVel_pinv(self.arm_chain) # Create a solver to retrieve the joint angles form the eef position self.IK_solver = PyKDL.ChainIkSolverPos_NR( self.arm_chain, self.forward_kinematics_solver, self.IK_v_solver)
def __init__(self): self.num_joints = 7 # Get the URDF (you will need to write your own "load_urdf()" function) urdf_path = os.path.join(assets_root, './kdl/panda_urdf/panda_arm_hand.urdf') # urdf_path= os.path.join(robosuite.models.assets_root, "bullet_data/sawyer_description/urdf/sawyer_intera.urdf") self.urdf_model = URDF.from_xml_string( open(urdf_path).read().encode( 'ascii')) # encode to ascii solve the error #self.kdl_tree = treeFromFile(urdf_path) # Set the base link, and the tip link. I think that these are just strings, defined somewhere in the URDF. The name 'right_hand' is how the gripper is described on the real Sawyer, but it may have a different name in your own URDF. self.base_link = self.urdf_model.get_root() self.tip_link = "panda_hand" # hand of panda robot, see panda_arm_hand.urdf # # Create a KDL tree from the URDF model. This is an object defining the kinematics of the entire robot. self.kdl_tree = treeFromUrdfModel(self.urdf_model) # Create a KDL chain from the tree. This is one specific chain within the overall kinematics model. self.arm_chain = self.kdl_tree[1].getChain(self.base_link, self.tip_link) # Create a solver which will be used to compute the forward kinematics self.forward_kinematics_solver = PyKDL.ChainFkSolverPos_recursive( self.arm_chain) # Create a solver which will be used to compute the Jacobian self.jacobian_solver = PyKDL.ChainJntToJacSolver(self.arm_chain) #Velocity inverse kinematics solver self.IK_v_solver = PyKDL.ChainIkSolverVel_pinv(self.arm_chain) # Create a solver to retrieve the joint angles form the eef position self.IK_solver = PyKDL.ChainIkSolverPos_NR( self.arm_chain, self.forward_kinematics_solver, self.IK_v_solver)
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._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, 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, urdf, base_link, end_link, kdl_tree=None): if kdl_tree is None: kdl_tree = kdl_tree_from_urdf_model(urdf) self.tree = kdl_tree self.urdf = urdf base_link = base_link.split("/")[-1] # for dealing with tf convention end_link = end_link.split("/")[-1] # for dealing with tf convention self.chain = kdl_tree.getChain(base_link, end_link) self.base_link = base_link self.end_link = end_link # record joint information in easy-to-use lists self.joint_limits_lower = [] self.joint_limits_upper = [] self.joint_safety_lower = [] self.joint_safety_upper = [] self.joint_types = [] for jnt_name in self.get_joint_names(): jnt = urdf.joints[jnt_name] if jnt.limits is not None: self.joint_limits_lower.append(jnt.limits.lower) self.joint_limits_upper.append(jnt.limits.upper) else: self.joint_limits_lower.append(None) self.joint_limits_upper.append(None) if jnt.safety is not None: self.joint_safety_lower.append(jnt.safety.lower) self.joint_safety_upper.append(jnt.safety.upper) elif jnt.limits is not None: self.joint_safety_lower.append(jnt.limits.lower) self.joint_safety_upper.append(jnt.limits.upper) else: self.joint_safety_lower.append(None) self.joint_safety_upper.append(None) self.joint_types.append(jnt.joint_type) def replace_none(x, v): if x is None: return v return x self.joint_limits_lower = np.array( [replace_none(jl, -np.inf) for jl in self.joint_limits_lower]) self.joint_limits_upper = np.array( [replace_none(jl, np.inf) for jl in self.joint_limits_upper]) self.joint_safety_lower = np.array( [replace_none(jl, -np.inf) for jl in self.joint_safety_lower]) self.joint_safety_upper = np.array( [replace_none(jl, np.inf) for jl in self.joint_safety_upper]) self.joint_types = np.array(self.joint_types) self.num_joints = len(self.get_joint_names()) self._fk_kdl = kdl.ChainFkSolverPos_recursive(self.chain) self._ik_v_kdl = kdl.ChainIkSolverVel_pinv(self.chain) self._ik_p_kdl = kdl.ChainIkSolverPos_NR(self.chain, self._fk_kdl, self._ik_v_kdl) self._jac_kdl = kdl.ChainJntToJacSolver(self.chain) self._dyn_kdl = kdl.ChainDynParam(self.chain, kdl.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): # Subscriptions self.objects = rospy.Subscriber('/found_objects', Object, self.callback_obj) self.tfBuffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tfBuffer) self.gp_action = actionlib.SimpleActionClient('request_trajectory', RequestTrajectoryAction) self.grasping_poses = [] self.object_list = [] self.old_list = [] self.joint_limits_lower = [] self.joint_limits_upper = [] self.joint_names = [] self.gp_weights = np.zeros(3) # Arm selection self.left_arm = False self.right_arm = False self.frame_end = '' self.desired_chain = 'left_chain' self.manip_threshold = 0.05 self.distance_threshold = 1.15 # Gripper frames: self.grip_left = 'left_gripper_tool_frame' # self.grip_right = 'left_gripper_tool_frame' self.grip_right = 'right_gripper_tool_frame' self.frame_base = 'base_link' # KDL chains: self.right_chain = kdl.Chain() self.left_chain = kdl.Chain() self.ik_lambda = 0.35 # how much should singularities be avoided? self.arms_chain() # generate chains for both arms self.left_jnt_pos = kdl.JntArray(self.nJoints) self.right_jnt_pos = kdl.JntArray(self.nJoints) self.jac_solver_left = kdl.ChainJntToJacSolver(self.left_chain) self.jac_solver_right = kdl.ChainJntToJacSolver(self.right_chain) self.joint_list = rospy.Subscriber('/joint_states', JointState, self.joint_callback) rospy.sleep(0.01)
def __init__(self): self._baxter = URDF.from_parameter_server(key='robot_description') self._kdl_tree = kdl_tree_from_urdf_model(self._baxter) self._base_link = self._baxter.get_root() self._tip_link = 'right_hand' self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) self.jac_solver = PyKDL.ChainJntToJacSolver(self._arm_chain) self.fk_solver = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
def __init__(self): rospy.init_node('goal_selector', anonymous=True) r = rospy.Rate(2) self.objects = rospy.Subscriber('/found_objects', Object, self.callback_obj) self.tfBuffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tfBuffer) self.grasping_poses = [] self.object_list = [] self.old_list =[] self.joint_limits_lower = [] self.joint_limits_upper = [] self.joint_names = [] self.gp_weights = np.zeros(6) # Arm selection self.left_arm = False self.right_arm = False self.frame_end = '' self.desired_chain = 'left_chain' self.manip_threshold = 0.05 self.distance_threshold = 1.15 # Gripper frames: self.grip_left = 'left_gripper_tool_frame' self.grip_right = 'right_gripper_tool_frame' self.frame_base = 'base_footprint' # KDL chains: self.right_chain = kdl.Chain() self.left_chain = kdl.Chain() self.ik_lambda = 0.35 # how much should singularities be avoided? self.arms_chain() # generate chains for both arms self.left_jnt_pos = kdl.JntArray(self.nJoints) self.right_jnt_pos = kdl.JntArray(self.nJoints) self.jac_solver_left = kdl.ChainJntToJacSolver(self.left_chain) self.jac_solver_right = kdl.ChainJntToJacSolver(self.right_chain) self.joint_list = rospy.Subscriber('/joint_states', JointState, self.joint_callback) rospy.sleep(0.5)
def __init__(self): super(YoubotKDL, self).__init__() # PyKDL self.kine_chain = PyKDL.Chain() self.setup_kdl_chain() self.current_joint_position = PyKDL.JntArray( self.kine_chain.getNrOfJoints()) self.current_pose = PyKDL.Frame() self.fk_solver = PyKDL.ChainFkSolverPos_recursive(self.kine_chain) self.ik_solver = PyKDL.ChainIkSolverPos_LMA(self.kine_chain) self.jac_calc = PyKDL.ChainJntToJacSolver(self.kine_chain)
def get_jacobian(self): # Obtain jacobian for the selected arm self.jac_solver = kdl.ChainJntToJacSolver(self.chain) jacobian = kdl.Jacobian(self.nJoints) self.jac_solver.JntToJac(self.joint_values_kdl, jacobian) jac_array = np.empty(0) for row in range(jacobian.rows()): for col in range(jacobian.columns()): jac_array = np.hstack((jac_array, jacobian[row, col])) jac_array = np.reshape(jac_array, (jacobian.rows(), jacobian.columns())) return jac_array
def prepare_kin_dyn(self): flag, self.tree = kdl_parser.treeFromFile("data/jelly/jelly.urdf") chain = self.tree.getChain(self.base_link, self.bl_link) self.fk_bl = kdl.ChainFkSolverPos_recursive(chain) self.jac_bl = kdl.ChainJntToJacSolver(chain) chain = self.tree.getChain(self.base_link, self.br_link) self.fk_br = kdl.ChainFkSolverPos_recursive(chain) self.jac_br = kdl.ChainJntToJacSolver(chain) chain = self.tree.getChain(self.base_link, self.fl_link) self.fk_fl = kdl.ChainFkSolverPos_recursive(chain) self.jac_fl = kdl.ChainJntToJacSolver(chain) chain = self.tree.getChain(self.base_link, self.fr_link) self.fk_fr = kdl.ChainFkSolverPos_recursive(chain) self.jac_fr = kdl.ChainJntToJacSolver(chain) # convention front_right, front_left, back_right, back_left self.jac_list = [self.jac_fl, self.jac_fr, self.jac_bl, self.jac_br] self.fk_list = [self.fk_fl, self.fk_fr, self.fk_bl, self.fk_br] joints = kdl.JntArray(3) joints[0] = 0 joints[1] = 0 joints[2] = 0 frame = kdl.Frame() jk_list = self.fk_list print(jk_list[0].JntToCart(joints, frame)) print(frame) print(jk_list[1].JntToCart(joints, frame)) print(frame) print(jk_list[2].JntToCart(joints, frame)) print(frame) print(jk_list[3].JntToCart(joints, frame)) print(frame)
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())
def _setup(self): # load in ros parameters self.baselink = rospy.get_param("sawyer/baselink") self.endlink = rospy.get_param("sawyer/endlink") flag, self.tree = kdl_parser.treeFromParam("/robot_description") self.joint_names = rospy.get_param("named_poses/right/joint_names") print self.joint_names # build kinematic chain and fk and jacobian solvers chain_ee = self.tree.getChain(self.baselink, self.endlink) self.fk_ee = kdl.ChainFkSolverPos_recursive(chain_ee) self.jac_ee = kdl.ChainJntToJacSolver(chain_ee) # building robot joint state self.num_joints = chain_ee.getNrOfJoints() self.joints = kdl.JntArray(self.num_joints)
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())