def __init__(self,robot,base_link,end_link,group, move_group_ns="move_group", planning_scene_topic="planning_scene", robot_ns="", verbose=False, kdl_kin=None, closed_form_IK_solver = None, joint_names=[]): self.robot = robot self.tree = kdl_tree_from_urdf_model(self.robot) self.chain = self.tree.getChain(base_link, end_link) if kdl_kin is None: self.kdl_kin = KDLKinematics(self.robot, base_link, end_link) else: self.kdl_kin = kdl_kin self.base_link = base_link self.joint_names = joint_names self.end_link = end_link self.group = group self.robot_ns = robot_ns self.client = actionlib.SimpleActionClient(move_group_ns, MoveGroupAction) self.verbose = verbose self.closed_form_IK_solver = closed_form_IK_solver self.closed_form_ur5_ik = InverseKinematicsUR5() # self.closed_form_ur5_ik.enableDebugMode() self.closed_form_ur5_ik.setEERotationOffsetROS() self.closed_form_ur5_ik.setJointWeights([6,5,4,3,2,1]) self.closed_form_ur5_ik.setJointLimits(-np.pi, np.pi)
def __init__(self, robot, base_link, end_link, group, move_group_ns="move_group", planning_scene_topic="planning_scene", robot_ns="", verbose=False, kdl_kin=None, closed_form_IK_solver=None, joint_names=[]): self.robot = robot self.tree = kdl_tree_from_urdf_model(self.robot) self.chain = self.tree.getChain(base_link, end_link) if kdl_kin is None: self.kdl_kin = KDLKinematics(self.robot, base_link, end_link) else: self.kdl_kin = kdl_kin self.base_link = base_link self.joint_names = joint_names self.end_link = end_link self.group = group self.robot_ns = robot_ns self.client = actionlib.SimpleActionClient(move_group_ns, MoveGroupAction) self.acceleration_magnification = 1 rospy.wait_for_service('compute_cartesian_path') self.cartesian_path_plan = rospy.ServiceProxy('compute_cartesian_path', GetCartesianPath) self.verbose = verbose self.closed_form_IK_solver = closed_form_IK_solver
def __init__(self, robot, base_link, end_link, group, move_group_ns="move_group", planning_scene_topic="planning_scene", robot_ns="", verbose=False, kdl_kin=None, closed_form_IK_solver=None, joint_names=[]): self.robot = robot self.tree = kdl_tree_from_urdf_model(self.robot) self.chain = self.tree.getChain(base_link, end_link) if kdl_kin is None: self.kdl_kin = KDLKinematics(self.robot, base_link, end_link) else: self.kdl_kin = kdl_kin self.base_link = base_link self.joint_names = joint_names self.end_link = end_link self.group = group self.robot_ns = robot_ns self.client = actionlib.SimpleActionClient(move_group_ns, MoveGroupAction) self.verbose = verbose self.closed_form_IK_solver = closed_form_IK_solver
def load_chain_from_URDF_string_and_joints(urdf_string, joint_list, target_robot=False): """ This function returns a kinmatic chain based on the URDF string given and the list of joints """ robot = URDF.from_xml_string(urdf_string) # check if joint is in it if not then expand both in child or parent base_link, end_link = "", "" for joint in joint_list: base_link, end_link = find_links_from_joints(joint, robot, base_link, end_link, target_robot=target_robot) urdf_logger.debug("Current base_link:{0}, Current end_link:{1}".format( base_link, end_link)) # return chain if base_link and end_link: tree = kdl_tree_from_urdf_model(robot) return tree.getChain(base_link, end_link) else: return None
def gravity_compensation_publisher(data): try: old except NameError: old = 0 if old <> data.position[1]: print 123 pub = rospy.Publisher('/panda_arm_controller/command', Float64MultiArray, queue_size=10) data_to_send = Float64MultiArray() robot = URDF.from_parameter_server() tree = kdl_tree_from_urdf_model(robot) chain = tree.getChain("panda_link0", "panda_link7") grav_vector = kdl.Vector(0, 0, -9.81) # relative to kdl chain base link dyn_kdl = kdl.ChainDynParam(chain, grav_vector) jt_positions = kdl.JntArray(7) jt_positions[0] = data.position[2] jt_positions[1] = data.position[3] jt_positions[2] = data.position[4] jt_positions[3] = data.position[5] jt_positions[4] = data.position[6] jt_positions[5] = data.position[7] jt_positions[6] = data.position[8] old = data.position[1] grav_matrix = kdl.JntArray(7) dyn_kdl.JntToGravity(jt_positions, grav_matrix) data_to_send.data = [grav_matrix[i] for i in range(grav_matrix.rows())] pub.publish(data_to_send)
def __init__(self): super(TomDataset, self).__init__("TOM") # hold bag file names self.trash_bags = [] self.box_bags = [] # hold demos self.box = [] self.trash = [] self.trash_poses = [] self.box_poses = [] self.move_poses = [] self.lift_poses = [] self.test_poses = [] self.pickup_poses = [] self.trash_data = [] self.box_data = [] self.move_data = [] self.lift_data = [] self.test_data = [] self.pickup_data = [] self.robot = URDF.from_parameter_server() self.base_link = "torso_link" self.end_link = "r_gripper_base_link" # set up kinematics stuff self.tree = kdl_tree_from_urdf_model(self.robot) self.chain = self.tree.getChain(self.base_link, self.end_link) self.kdl_kin = KDLKinematics(self.robot, self.base_link, self.end_link)
def get_jacabian_from_joint(self, urdfname, jointq): robot = URDF.from_xml_file(urdfname) tree = kdl_tree_from_urdf_model(robot) # print tree.getNrOfSegments() chain = tree.getChain("base_link", "ee_link") # print chain.getNrOfJoints() # forwawrd kinematics kdl_kin = KDLKinematics(robot, "base_link", "ee_link") q = jointq #q = [0, 0, 1, 0, 1, 0] pose = kdl_kin.forward( q) # forward kinematics (returns homogeneous 4x4 matrix) # # print pose # #print list(pose) # q0=Kinematic(q) # if flag==1: # q_ik=q0.best_sol_for_other_py( [1.] * 6, 0, q0.Forward()) # else: # q_ik = kdl_kin.inverse(pose) # inverse kinematics # # print "----------iverse-------------------\n", q_ik # # if q_ik is not None: # pose_sol = kdl_kin.forward(q_ik) # should equal pose # print "------------------forward ------------------\n",pose_sol J = kdl_kin.jacobian(q) #print 'J:', J return J, pose
def get_jacabian_from_joint(self, urdfname, jointq, flag): #robot = URDF.from_xml_file("/data/ros/ur_ws/src/universal_robot/ur_description/urdf/ur5.urdf") robot = URDF.from_xml_file(urdfname) tree = kdl_tree_from_urdf_model(robot) # print tree.getNrOfSegments() chain = tree.getChain("base_link", "ee_link") # print chain.getNrOfJoints() # forwawrd kinematics kdl_kin = KDLKinematics(robot, "base_link", "ee_link") q = jointq #q = [0, 0, 1, 0, 1, 0] pose = kdl_kin.forward( q) # forward kinematics (returns homogeneous 4x4 matrix) # print pose #print list(pose) q0 = Kinematic() if flag == 1: q_ik = q0.best_sol_for_other_py([1.] * 6, 0, q0.Forward(q)) else: q_ik = kdl_kin.inverse(pose) # inverse kinematics # print "----------iverse-------------------\n", q_ik if q_ik is not None: pose_sol = kdl_kin.forward(q_ik) # should equal pose print "------------------forward ------------------\n", pose_sol J = kdl_kin.jacobian(q) #print 'J:', J return J, pose
def __init__(self, config): base_link = config['base_link'] end_link = config['end_link'] if 'robot_description_param' in config: self.robot = URDF.from_parameter_server( config['robot_description_param']) else: self.robot = URDF.from_parameter_server() self.config = config self.base_link = base_link # set up kinematics stuff self.tree = kdl_tree_from_urdf_model(self.robot) self.chain = self.tree.getChain(base_link, end_link) self.kdl_kin = KDLKinematics(self.robot, base_link, end_link) self.base_link = base_link self.end_link = end_link self.skill_instances = {} self.skill_features = {} self.skill_models = {} self.parent_skills = {} self.pubs = {}
def __init__(self, robot, base_link, end_link, group, move_group_ns="move_group", planning_scene_topic="planning_scene", robot_ns="", verbose=False, kdl_kin=None, closed_form_IK_solver = None, joint_names=[]): self.robot = robot self.tree = kdl_tree_from_urdf_model(self.robot) self.chain = self.tree.getChain(base_link, end_link) if kdl_kin is None: self.kdl_kin = KDLKinematics(self.robot, base_link, end_link) else: self.kdl_kin = kdl_kin self.base_link = base_link self.joint_names = joint_names self.end_link = end_link self.group = group self.robot_ns = robot_ns self.client = actionlib.SimpleActionClient(move_group_ns, MoveGroupAction) self.acceleration_magnification = 1 rospy.wait_for_service('compute_cartesian_path') self.cartesian_path_plan = rospy.ServiceProxy('compute_cartesian_path',GetCartesianPath) self.verbose = verbose self.closed_form_IK_solver = closed_form_IK_solver
def __init__(self, urdf_file_name, base_link="base_link", ee_name=_EE_NAME): ''' Simple model of manipulator kinematics and controls Assume following state and action vectors urdf_file_name - model file to load ''' # Load KDL tree urdf_file = file(urdf_file_name, 'r') self.robot = Robot.from_xml_string(urdf_file.read()) urdf_file.close() self.tree = kdl_tree_from_urdf_model(self.robot) task_space_ik_weights = np.diag([1.0, 1.0, 1.0, 0.0, 0.0, 0.0]).tolist() #self.base_link = self.robot.get_root() self.base_link = base_link self.joint_chains = [] self.chain = KDLKinematics(self.robot, self.base_link, ee_name) '''
def __init__(self): self.robot = self.init_robot( "/home/zy/catkin_ws/src/aubo_robot/aubo_description/urdf/aubo_i5.urdf" ) self.kdl_kin = KDLKinematics(self.robot, "base_link", "wrist3_Link") self.tree = kdl_tree_from_urdf_model(self.robot) self.chain = self.tree.getChain("base_link", "wrist3_Link") self.q = [0, 0, 0, 0, 0, 0]
def get_jacabian_from_joint(self, urdfname, jointq, flag): robot = URDF.from_xml_file(urdfname) tree = kdl_tree_from_urdf_model(robot) chain = tree.getChain("base_link", "ee_link") kdl_kin = KDLKinematics(robot, "base_link", "ee_link") J = kdl_kin.jacobian(jointq) pose = kdl_kin.forward(jointq) return J, pose
def get_data_from_kdl(self): robot = URDF.from_xml_file(self.urdfname) tree = kdl_tree_from_urdf_model(robot) chain = tree.getChain("base_link", "ee_link") # print chain.getNrOfJoints() # forwawrd kinematics kdl_kin = KDLKinematics(robot, "base_link", "ee_link") return kdl_kin
def __init__(self): print "建立机器人模型" # 建立机器人模型参数 robot_urdf = URDF.from_xml_file( "/home/d/catkin_ws/src/robot_description/armc_description/urdf/armc_description.urdf" ) tree = kdl_tree_from_urdf_model(robot_urdf) super(MyKdlByUrdf, self).__init__(robot_urdf, "base_link", "sensor_link", tree)
def __init__(self): print "Instantiating..." self._joint_vec = np.zeros(6) self._rate = rospy.Rate(10) self._pc = None self._pc_prev = np.array([0, 0, 0]) self._R_capsule_world = None self._capsule_lin_vel = None self._capsule_ang_vel = None self._pa = None self._pa_prev = np.array([0, 0, 0]) self._R_EPM_world = None self._fm = np.zeros(3) self._fm_prev = np.zeros(3) self._tm = np.zeros(3) self._tm_prev = np.zeros(3) self._coupling_status_msg = Float64() self._df_dx_msg = Vector3() self._df_dx_raw_msg = Vector3() self._dcapz_dEPMz_msg = Vector3() self._dcapz_dEPMz_raw_msg = Vector3() self._mag_wrench_msg = Wrench() # print "To publish: " , self._to_publish self._dipole = DipoleField(1.48, 1.48, SC.ACTUATOR_MAG_H, SC.CAPSULE_MAG_H) # print "Mag info:", SC.ACTUATOR_MAG_H # Initial conditions for Butterworth fulter fs = 100.0 nyq = 0.5 * fs cutoff = 0.5 order = 2 self._butter_zi_dfdx = np.zeros((3, order)) self._butter_params_dfdx = butter(order, cutoff / nyq, btype="lowpass", analog=False) self._butter_zi_dz = np.zeros((3, order)) self._butter_params_dz = butter(order, cutoff / nyq, btype="lowpass", analog=False) # Instantiate publishers and subscribers self._coupling_status_pub = rospy.Publisher("/MAC/coupling_status_topic", Float64, queue_size=1000) self._df_dx_pub = rospy.Publisher("/MAC/df_dx_topic", Vector3, queue_size=1000) self._df_dx_raw_pub = rospy.Publisher("/MAC/df_dx_raw_topic", Vector3, queue_size=1000) self._mag_wrench_pub = rospy.Publisher("/MAC/mag_wrench_topic", Wrench, queue_size=1000) self._dcapz_dEPMz_pub = rospy.Publisher("/MAC/dcapz_dEPMz_topic", Vector3, queue_size=1000) self._dcapz_dEPMz_raw_pub = rospy.Publisher("/MAC/dcapz_dEPMz_raw_topic", Vector3, queue_size=1000) self._robot_sub = rospy.Subscriber("/mitsubishi_arm/joint_states", JointState, self._robot_pose_cb) self._capsule_sub = rospy.Subscriber("/MAC/mac/odom", Odometry, self._capsule_pose_cb) # Robot info self._robot = self._wait_and_get_robot() self._tree = kdl_tree_from_urdf_model(self._robot) self._chain_to_magnet = self._tree.getChain("base_link", "magnet_center") self._fksolver_magnet = KDL.ChainFkSolverPos_recursive(self._chain_to_magnet) self._q_cur = KDL.JntArray(self._chain_to_magnet.getNrOfJoints()) # Initialize timer pub_timer = rospy.Timer(rospy.Duration(0.01), self._determine_coupling_state)
def main(): robot = URDF.from_parameter_server() print robot.get_root() tree = kdl_tree_from_urdf_model(robot) print tree.getNrOfSegments() chain = tree.getChain(robot.get_root(), 'right_gripper_r_finger_tip') print chain.getNrOfJoints() init_pos = [ 1.605525390625, -3.06816015625, 1.708900390625, 0.0580078125, 1.364841796875, 0.12111677875185967, 0.721302320838, 0.036830651785714284 ] jnt_indices = [3, 5] q_jnt_angles = copy.copy(init_pos) l_pos_limit = -2.0 u_pos_limit = 2.0 kdl_kin_r = KDLKinematics(robot, robot.get_root(), 'right_gripper_r_finger_tip') kdl_kin_l = KDLKinematics(robot, robot.get_root(), 'right_gripper_l_finger_tip') print kdl_kin_l.get_joint_names() print kdl_kin_l.get_joint_limits() #kdl_kin_r.extract_joint_state(js) ''' for index in jnt_indices: # Do a -1 on the index because we are indexing into # a list that does not include the sawyer head pan # index includes the head pan q_jnt_angles[index] = np.random.uniform(l_pos_limit, u_pos_limit) ''' print q_jnt_angles # forward kinematics (returns homogeneous 4x4 numpy matrix) pose_r = kdl_kin_r.forward(q_jnt_angles) pose_l = kdl_kin_l.forward(q_jnt_angles) print "pose_l: ", pose_l print "pose_r: ", pose_r pose = 0.5 * (pose_l + pose_r) x, y, z = transformations.translation_from_matrix(pose)[:3] print x, y, z q_ik_l = kdl_kin_l.inverse(pose_l, q_jnt_angles) # inverse kinematics if q_ik_l is not None: pose_sol = kdl_kin_l.forward(q_ik_l) # should equal pose print 'q_ik_l:', q_ik_l print 'pose_sol:', pose_sol q_ik_r = kdl_kin_r.inverse(pose_r, q_jnt_angles) # inverse kinematics if q_ik_r is not None: pose_sol = kdl_kin_r.forward(q_ik_r) # should equal pose print 'q_ik_r:', q_ik_r print 'pose_sol:', pose_sol
def talker(): rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10000) file_name = "/home/zy/catkin_ws/src/paintingrobot_related/paintingrobot_underusing/paintingrobot_description/urdf/base/paintingrobot_description_witharm.urdf" marker_pub = rospy.Publisher("visualization_marker", Marker, queue_size=10) visualization_num = 1 N = 500000 q = np.zeros(11) pyplot_test() while not rospy.is_shutdown(): robot = URDF.from_xml_file(file_name) kdl_kin = KDLKinematics(robot, "base_link", "tool0") tree = kdl_tree_from_urdf_model(robot) chain = tree.getChain("base_link", "tool0") q[0] = 0.0 q[1] = 0.0 q[2] = 0.0 q[3] = -0.1 + 1.1 * random.random() q[4] = -0.5 + (0.5 + 3 / 4.0 * math.pi) * random.random() q[5] = -1 / 6 * math.pi + (1 / 6 + 175 / 180.0) * math.pi * random.random() q[6] = -175 / 180.0 * math.pi + 2 * 175 / 180.0 * math.pi * random.random( ) q[7] = -175 / 180.0 * math.pi + 2 * 175 / 180.0 * math.pi * random.random( ) q[8] = -175 / 180.0 * math.pi + 2 * 175 / 180.0 * math.pi * random.random( ) q[9] = -175 / 180.0 * math.pi + 2 * 175 / 180.0 * math.pi * random.random( ) q[10] = -175 / 180.0 * math.pi + 2 * 175 / 180.0 * math.pi * random.random( ) print("the joints are:", q) pose = kdl_kin.forward(q) # print "pose is:", pose[0,3], pose[1,3], pose[2,3] mobileplatform_targepositions = np.array( [pose[0, 3], pose[1, 3], pose[2, 3], 1.0, 0.0, 0.0, 0.0]) # frame = 'base_link' # scale1=np.array([0.05,0.05,0.05]) # color1=np.array([0.0,1.0,0.0]) # marker1,visualization_num=targetpositions_visualization(mobileplatform_targepositions, frame, visualization_num, scale1, color1) # marker_pub.publish(marker1) # visualization_num=visualization_num+1 # print("visualization_num is:",visualization_num) if visualization_num > N: break rate.sleep()
def __init__(self): # rospy.init_node("import_ur3_from_urdf") self.robot = self.init_robot( "/data/ros/ur_ws/src/universal_robot/ur5_planning/urdf/ur3.urdf") self.kdl_kin = KDLKinematics(self.robot, "base_link", "ee_link") self.tree = kdl_tree_from_urdf_model(self.robot) self.chain = self.tree.getChain("base_link", "ee_link") # safe angle of UR3 self.safe_q = [ 1.3189744444444444, -2.018671111111111, 1.8759755555555557, 2.7850055555555557, 0.17444444444444443, 3.7653833333333337 ] self.q = self.safe_q
def loadKinematicsFromURDF(self, filename, base_link): ''' Load KDL kinematics class for easy lookup of posiitons from the urdf model of a particular robot. Params: -------- filename: absolute path to URDF file base_link: root of kinematic tree ''' urdf = URDF.from_xml_file(filename) tree = kdl_tree_from_urdf_model(urdf) chain = tree.getChain(base_link, self.grasp_link) self.kinematics = KDLKinematics(urdf, base_link, self.grasp_link)
def __init__(self): # rospy.init_node("import_ur3_from_urdf") self.robot = self.init_robot( "/data/ros/yue_ws_201903/src/tcst_pkg/urdf/ur5.urdf") self.kdl_kin = KDLKinematics(self.robot, "base_link", "ee_link") self.tree = kdl_tree_from_urdf_model(self.robot) self.chain = self.tree.getChain("base_link", "ee_link") # safe angle of UR3 self.safe_q = [ 0.20741444444444448, -1.8128266666666668, -1.2626288888888888, -1.66891, 1.6191933333333333, 3.204893333333333 ] self.q = self.safe_q
def __init__(self): # rospy.init_node("import_aubo5_from_urdf") self.robot = self.init_robot( "/home/zy/catkin_ws/src/aubo_robot/aubo_description/urdf/aubo_i5.urdf" ) self.kdl_kin = KDLKinematics(self.robot, "base_link", "wrist3_Link") self.tree = kdl_tree_from_urdf_model(self.robot) self.chain = self.tree.getChain("base_link", "wrist3_Link") # safe angle of UR3 self.safe_q = [ 1.3189744444444444, -2.018671111111111, 1.8759755555555557, 2.7850055555555557, 0.17444444444444443, 3.7653833333333337 ] self.q = self.safe_q
def setUpClass(cls): """ get_some_resource() is slow, to avoid calling it for each test use setUpClass() and store the result as class variable """ super(TestMethods, cls).setUpClass() target_robot = URDF.from_xml_file( '/home/{0}/ros_examples/configs/ur5.urdf'.format( getpass.getuser())) target_tree = kdl_tree_from_urdf_model(target_robot) base_link = 'base_link' end_link = 'ee_link' cls.target = target_tree.getChain(base_link, end_link) cls.angles = [0, 0, 0, 0, 0, 0] cls.eps = forwardKinematics(cls.target, cls.angles) """
def setUpClass(cls): """ get_some_resource() is slow, to avoid calling it for each test use setUpClass() and store the result as class variable """ super(TestMethods, cls).setUpClass() target_robot = URDF.from_xml_file( os.path.dirname(os.path.abspath(__file__)) + '/../config_files/urdf/ur5.urdf') target_tree = kdl_tree_from_urdf_model(target_robot) base_link = 'base_link' end_link = 'ee_link' cls.target = target_tree.getChain(base_link, end_link) cls.angles = [0, 0, 0, 0, 0, 0] cls.eps = forwardKinematics(cls.target, cls.angles) """
def get_jacabian_from_joint(self, urdfname, jointq, flag): robot = URDF.from_xml_file(urdfname) tree = kdl_tree_from_urdf_model(robot) chain = tree.getChain("base_link", "ee_link") kdl_kin = KDLKinematics(robot, "base_link", "ee_link") q = jointq pose = kdl_kin.forward(q) q0 = Kinematic() if flag == 1: q_ik = q0.best_sol_for_other_py([1.] * 6, 0, q0.Forward(q)) else: q_ik = kdl_kin.inverse(pose) if q_ik is not None: pose_sol = kdl_kin.forward(q_ik) J = kdl_kin.jacobian(q) return J, pose
def kdl_kinematics (self,data): self.q_sensors = data self.tree = kdl_tree_from_urdf_model(self.omni) # create a kdl tree from omni URDF model self.omni_kin = KDLKinematics(self.omni, "base", "stylus") # create instance that captures the kinematics of the robot arm #Forward Kinematics self.pose_stylus = self.omni_kin.forward(data) #compute the forward kinematics from the sensor joint angle position using the kinematics from the kdl tree #Inverse Kinematics self.q_guess = numpy.array(data)+0.2 #make an initial guess for your joint angles by deviating all the sensor joint angles by 0.2 self.q_ik = self.omni_kin.inverse(self.pose_stylus, self.q_guess) #using the position from the forward kinematics 'pose_stylus' and the offset initial guess, compute #the desired joint angles for that position. self.delta_q = self.q_ik-data
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 get_jacabian_from_joint(self, urdfname, jointq, flag): #robot = URDF.from_xml_file("/data/ros/ur_ws/src/universal_robot/ur_description/urdf/ur5.urdf") robot = URDF.from_xml_file(urdfname) tree = kdl_tree_from_urdf_model(robot) # print tree.getNrOfSegments() chain = tree.getChain("base_link", "tool0") # print chain.getNrOfJoints() # forwawrd kinematics kdl_kin = KDLKinematics(robot, "base_link", "tool0") q = jointq #q = [0, 0, 1, 0, 1, 0] pose = kdl_kin.forward( q) # forward kinematics (returns homogeneous 4x4 matrix) J = kdl_kin.jacobian(q) #print 'J:', J return J, pose
def __init__(self): self.a2 = 0.408 self.a3 = 0.376 self.d1 = 0.122 self.d2 = 0.1215 self.d5 = 0.1025 self.d6 = 0.094 self.ZERO_THRESH = 1e-4 self.ARM_DOF = 6 self.robot = self.init_robot( "/home/zy/catkin_ws/src/aubo_robot/aubo_description/urdf/aubo_i5.urdf" ) self.kdl_kin = KDLKinematics(self.robot, "base_link", "wrist3_Link") self.tree = kdl_tree_from_urdf_model(self.robot) self.chain = self.tree.getChain("base_link", "wrist3_Link") self.q = [0, 0, 0, 0, 0, 0]
def __init__(self, side, step, x_min, x_max, y_min, y_max, z_min, z_max, pt_c_in_T2, min_dist, max_dist, rot): self.min_dist2 = min_dist*min_dist self.max_dist2 = max_dist*max_dist self.x_set = list(np.arange(x_min, x_max, step)) self.y_set = list(np.arange(y_min, y_max, step)) if z_min > z_max: self.z_set = list(np.arange(z_min, z_max, -step)) else: self.z_set = list(np.arange(z_min, z_max, step)) self.pt_c_in_T2 = pt_c_in_T2 self.rot = rot self.robot = URDF.from_parameter_server() self.tree = kdl_tree_from_urdf_model(self.robot) self.chain = self.tree.getChain("torso_link2", side + "_HandPalmLink") self.q_min = PyKDL.JntArray(7) self.q_max = PyKDL.JntArray(7) self.q_limit = 0.01 self.q_min[0] = -2.96 + self.q_limit self.q_min[1] = -2.09 + self.q_limit self.q_min[2] = -2.96 + self.q_limit # self.q_min[3] = -2.09 + self.q_limit self.q_min[3] = 15.0/180.0*math.pi self.q_min[4] = -2.96 + self.q_limit self.q_min[5] = -2.09 + self.q_limit self.q_min[6] = -2.96 + self.q_limit self.q_max[0] = 2.96 - self.q_limit # self.q_max[1] = 2.09 - self.q_limit self.q_max[1] = -15.0/180.0*math.pi self.q_max[2] = 2.96 - self.q_limit self.q_max[3] = 2.09 - self.q_limit self.q_max[4] = 2.96 - self.q_limit # self.q_max[5] = 2.09 - self.q_limit self.q_max[5] = -15.0/180.0*math.pi self.q_max[6] = 2.96 - self.q_limit self.fk_solver = PyKDL.ChainFkSolverPos_recursive(self.chain) self.vel_ik_solver = PyKDL.ChainIkSolverVel_pinv(self.chain) self.ik_solver = PyKDL.ChainIkSolverPos_NR_JL(self.chain, self.q_min, self.q_max, self.fk_solver, self.vel_ik_solver, 100) self.q_out = PyKDL.JntArray(7) self.singularity_angle = 15.0/180.0*math.pi
def jacobian_generation(q): robot = URDF.from_xml_file( "/home/zy/catkin_ws/src/aubo_robot-master/aubo_description/urdf/aubo_i5.urdf" ) tree = kdl_tree_from_urdf_model(robot) # print tree.getNrOfSegments() # chain = tree.getChain("base_link", "wrist3_Link") # print chain.getNrOfJoints() # forwawrd kinematics kdl_kin = KDLKinematics(robot, "base_link", "wrist3_Link") pose = kdl_kin.forward( q) # forward kinematics (returns homogeneous 4x4 matrix) # print pose # print type(pose) J = kdl_kin.jacobian(q) # print 'J:', J # print len(J) return J
def __init__(self): self.robot = URDF.from_xml_file("/home/you/abb/590.urdf") self.tree = kdl_tree_from_urdf_model(self.robot) # print self.tree.getNrOfSegments() self.chain = self.tree.getChain("base", "tool0") # print self.chain.getNrOfJoints() self.kdl_kin = KDLKinematics(self.robot, "base_link", "link_6", self.tree) self.steps = 0 self.done = False self.target_position = [0.5] * 3 self.r = 0.0 self.success_dist = 0.08 self.success_dist_min = 0.08 self.link = 'link_6' self.distance = [0.0] * 3 self.random_pos = [] self.a_bound = [2.9, 1.88, 2.2] self.offset = [0.0, 0.20, -1.1] # self.a_bound = [0.0, 1.88, 2.2] # self.offset = [0.0, 0.20, -1.1] self.reference_frame = 'base_link'
def configure(self, tf_base_link_name, tf_end_link_name): self.tf_base_link_name = tf_base_link_name self.tf_end_link_name = tf_end_link_name #Variables holding the joint information self.robot = URDF.from_parameter_server() self.tree = kdl_tree_from_urdf_model(self.robot) self.chain = self.tree.getChain(self.tf_base_link_name, self.tf_end_link_name) self.kdl_kin = KDLKinematics(self.robot, self.tf_base_link_name, self.tf_end_link_name) self.arm_joint_names = self.kdl_kin.get_joint_names() self.jnt_pos = kdl.JntArray(self.chain.getNrOfJoints()) self.fk_solver = kdl.ChainFkSolverPos_recursive(self.chain) self.ikv_solver = kdl.ChainIkSolverVel_wdls(self.chain) self.ikv_solver.setLambda(0.0001) #self.ikv_solver.setWeightTS([[1,0,0,0,0,0],[0,1,0,0,0,0],[0,0,0,0,0,0],[0,0,0,1,0,0],[0,0,0,0,1,0],[0,0,0,0,0,1]]) self.ik_solver = kdl.ChainIkSolverPos_NR(self.chain, self.fk_solver, self.ikv_solver)
def get_jacabian_from_joint(): robot = URDF.from_xml_file("/data/ros/yue_ws/src/tcst_pkg/urdf/ur5.urdf") # robot = URDF.from_xml_file(urdfname) tree = kdl_tree_from_urdf_model(robot) # print tree.getNrOfSegments() chain = tree.getChain("base_link", "tool0") # print chain.getNrOfJoints() # forwawrd kinematics kdl_kin = KDLKinematics(robot, "base_link", "tool0") # q=jointq q = [44.87, -70.67, 40.06, 299.46, -89.69, 175.71] q = degree_to_rad(q) q = [ 2.4146897759547734, -1.1350234765509686, 0.5554777536842516, 5.258265530917523, -1.5663199722357237, 3.1563836819776188 ] pose = kdl_kin.forward( q) # forward kinematics (returns homogeneous 4x4 matrix) print "pose-----", pose q_ik = kdl_kin.inverse(pose) # inverse kinematics print "----------iverse-------------------\n", q_ik # print(pose) #print list(pose) # q0=Kinematic() # print(q0.Forward(q)) # if flag==1: # q_ik=q0.best_sol_for_other_py( [1.] * 6, 0, q0.Forward(q)) # else: # q_ik = kdl_kin.inverse(pose) # inverse kinematics # print "----------iverse-------------------\n", q_ik # if q_ik is not None: # pose_sol = kdl_kin.forward(q_ik) # should equal pose # print "------------------forward ------------------\n",pose_sol J = kdl_kin.jacobian(q) print('kdl J:', J) return J, pose
def main(): global tf global command, limbs global pub # Initialize the ROS node rospy.init_node('kdl_kinect') # Create a transform listener tf = TransformListener() # Retrieve raw robot parameters from rosmaster robot_string = rospy.get_param("robot_description", None) if not robot_string: raise Exception('Robot model not specified') # Load URDF model of robot description locally robot_urdf = URDF.parse_xml_string(robot_string) # Load URDF model of robot description into KDL robot_kdl = kdl_parser.kdl_tree_from_urdf_model(robot_urdf) # Publish Atlas commands pub = rospy.Publisher('/atlas/atlas_command', AtlasCommand) # Subscribe to hydra and atlas updates rospy.Subscriber("/arms/hydra_calib", Hydra, hydra_callback, queue_size = 1) rospy.Subscriber("/skeleton", Skeleton, skeleton_callback, queue_size = 1) rospy.Subscriber("/atlas/atlas_state", AtlasState, atlas_callback, queue_size = 1) # Start main event handling loop rospy.loginfo('Started kdl_hydra node...') r = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): r.sleep() rospy.loginfo('Stopping kdl_hydra node...')
def __init__(self, base_link, end_link, planning_group, world="/world", listener=None, traj_step_t=0.1, max_acc=1, max_vel=1, max_goal_diff = 0.02, goal_rotation_weight = 0.01, max_q_diff = 1e-6, start_js_cb=True, base_steps=10, steps_per_meter=100): self.world = world self.base_link = base_link self.end_link = end_link self.planning_group = planning_group self.base_steps = base_steps self.steps_per_meter = steps_per_meter self.MAX_ACC = max_acc self.MAX_VEL = max_vel self.traj_step_t = traj_step_t self.max_goal_diff = max_goal_diff self.max_q_diff = max_q_diff self.goal_rotation_weight = goal_rotation_weight self.at_goal = True self.near_goal = True self.moving = False self.q0 = [0,0,0,0,0,0,0] self.old_q0 = [0,0,0,0,0,0,0] self.cur_stamp = 0 self.teach_mode = rospy.Service('/costar/SetTeachMode',SetTeachMode,self.set_teach_mode_call) self.servo_mode = rospy.Service('/costar/SetServoMode',SetServoMode,self.set_servo_mode_call) self.shutdown = rospy.Service('/costar/ShutdownArm',EmptyService,self.shutdown_arm_call) self.servo = rospy.Service('/costar/ServoToPose',ServoToPose,self.servo_to_pose_call) self.plan = rospy.Service('/costar/PlanToPose',ServoToPose,self.plan_to_pose_call) self.smartmove = rospy.Service('/costar/SmartMove',SmartMove,self.smart_move_call) self.js_servo = rospy.Service('/costar/ServoToJointState',ServoToJointState,self.servo_to_joints_call) self.pt_publisher = rospy.Publisher('/joint_traj_pt_cmd',JointTrajectoryPoint,queue_size=1000) self.get_waypoints_srv = GetWaypointsService(world=world,service=False) self.driver_status = 'IDLE' self.status_publisher = rospy.Publisher('/costar/DriverStatus',String,queue_size=1000) self.robot = URDF.from_parameter_server() if start_js_cb: self.js_subscriber = rospy.Subscriber('joint_states',JointState,self.js_cb) self.tree = kdl_tree_from_urdf_model(self.robot) self.chain = self.tree.getChain(base_link, end_link) if listener is None: self.listener = tf.TransformListener() else: self.listener = listener #print self.tree.getNrOfSegments() #print self.chain.getNrOfJoints() self.kdl_kin = KDLKinematics(self.robot, base_link, end_link) self.display_pub = rospy.Publisher('costar/display_trajectory',DisplayTrajectory,queue_size=1000) #self.set_goal(self.q0) self.goal = None self.ee_pose = None self.planner = SimplePlanning(self.robot,base_link,end_link,self.planning_group)
import rospy from urdf_parser_py.urdf import URDF #from urdf_parser_py.urdf import Robot #from urdfdom_py.urdf import URDF from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model from pykdl_utils.kdl_kinematics import KDLKinematics # VERSION CHANGE - http://answers.ros.org/question/197609/how-to-read-a-urdf-in-python-in-indigo/ #robot = URDF.load_from_parameter_server(verbose=False) robot = URDF.from_parameter_server() base_link = robot.get_root() end_link = "link_6" #robot.link_map.keys()[len(robot.link_map)-1] # robot.links[6] print end_link tree = kdl_tree_from_urdf_model(robot) print tree.getNrOfSegments() chain = tree.getChain(base_link, end_link) print chain.getNrOfJoints() kdl_kin = KDLKinematics(robot, base_link, end_link) q = kdl_kin.random_joint_angles() print 'joints:', q pose = kdl_kin.forward(q) # forward kinematics (returns homogeneous 4x4 numpy.mat) print 'pose:', pose #q_ik = kdl_kin.inverse(pose, q+0.3) # inverse kinematics #if q_ik is not None: # pose_sol = kdl_kin.forward(q_ik) # should equal pose #J = kdl_kin.jacobian(q) #print 'q:', q