예제 #1
0
def runDiff(original_file, new_file, output_file):
    original_robot = URDF.from_xml_file(original_file)
    new_robot = URDF.from_xml_file(new_file)
    # only joint and link are considered
    diffs = dict()
    for j in original_robot.joints:
        new_j = findJointByName(new_robot, j.name)
        # check origin difference
        if new_j:
            diff = jointDiff(j, new_j)
            if diff:
                diffs[j.name] = diff
    with open(output_file, "w") as f:
        f.write(yaml.dump(diffs))
        print(yaml.dump(diffs))
예제 #2
0
def runDiff(original_file, new_file, output_file):
    original_robot = URDF.from_xml_file(original_file)
    new_robot = URDF.from_xml_file(new_file)
    # only joint and link are considered
    diffs = dict()
    for j in original_robot.joints:
        new_j = findJointByName(new_robot, j.name)
        # check origin difference
        if new_j:
            diff = jointDiff(j, new_j)
            if diff:
                diffs[j.name] = diff
    with open(output_file, "w") as f:
        f.write(yaml.dump(diffs))
        print yaml.dump(diffs)
예제 #3
0
    def __init__(self,q_start):
        rospy.init_node('null_space_test', anonymous=True)

        self.robot = URDF.from_xml_file("/data/ros/yue_ws/src/tcst_pkg/urdf/ur5.urdf")
        self.urscript_pub=rospy.Publisher("/ur_driver/URScript",String,queue_size=10)

        self.error_r_pub=rospy.Publisher("/null_space/error_r",Vector3,queue_size=10)
        self.rdes_pub=rospy.Publisher("/null_space/rdes",Vector3,queue_size=10)
        self.r_pub=rospy.Publisher("/null_space/r",Vector3,queue_size=10)
        self.condition_num_pub=rospy.Publisher("/null_space/condition_num",Float64,queue_size=10)

        self.q4_pub=rospy.Publisher("/null_space/q4",Float64,queue_size=10)
        self.q6_pub=rospy.Publisher("/null_space/q6",Float64,queue_size=10)


        self.aubo_q=q_start#[1.50040841e-03, -2.83640237e+00, 1.53798406e+00, 1.29841831e+00, 1.50040840e-03, 3.14159265e+00]
        # self.aubo_q=[0.4169788306196942, -1.3621199297826534, -2.011464437502717, -2.22014083451496, -1.5707963267948966, 1.1538174961752024]
        self.aubo_q1=np.matrix(self.aubo_q)
        
        self.vel=1.05
        self.ace=1.4
        self.t=0

        self.kpr=0.3
        self.kq4=0.1
        self.kq6=1000 
        self.bq4=pi/20
        self.bq6=pi/20
예제 #4
0
    def __init__(self, limb, ee_frame_name="panda_link8", additional_segment_config=None, description=None):

        self._kdl_tree = 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:
            # add additional segments to account for NE_T_EE and F_T_NE transformations
            # ---- this may cause issues in eg. inertia computations maybe? TODO: test inertia behaviour
            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_frame = PyKDL.Frame()
        self._limb_interface = limb
        self.create_chain(ee_frame_name)
예제 #5
0
    def __init__(self, urdf_path, sem_path, name_suffix=None):
        self.nsmap = {
            "xsd": "http://www.w3.org/2001/XMLSchema#",
            "owl": "http://www.w3.org/2002/07/owl#",
            "xsd": "http://www.w3.org/2001/XMLSchema#",
            "srdl2": "http://knowrob.org/kb/srdl2.owl#",
            "owl2xml": "http://www.w3.org/2006/12/owl2-xml#",
            "knowrob": "http://knowrob.org/kb/knowrob.owl#",
            "rdfs": "http://www.w3.org/2000/01/rdf-schema#",
            "rdf": "http://www.w3.org/1999/02/22-rdf-syntax-ns#",
            "srdl2-comp": "http://knowrob.org/kb/srdl2-comp.owl#",
            "srdl2-cap": "http://knowrob.org/kb/srdl2-cap.owl#",
            "qudt-unit": "http://qudt.org/vocab/unit#",
        }
        self.imports = [
            "package://knowrob_srdl/owl/srdl2-comp.owl",
            "package://knowrob_common/owl/knowrob.owl",
        ]
        self.id_gen = UniqueStringGenerator()
        self.urdf = URDF.from_xml_file(urdf_path)
        self.urdf.check_valid()

        basename = os.path.basename(sem_path)
        namespace, _ = os.path.splitext(basename)
        self.map_ns = namespace
        if name_suffix is None:
            self.map_name = self.urdf.name + "_" + self.id_gen.gen()
        else:
            self.map_name = self.urdf.name + "_" + name_suffix
        self.map_uri_base = "http://knowrob.org/kb/%s" % basename
        self.map_uri = self.map_uri_base + "#"
        self.nsmap[self.map_ns] = self.map_uri
        self.transformations = {}
    def __init__(self, rate=100):

        robot = URDF.from_xml_file(
            "/home/pedge/catkin_ws/src/baxter_force_control/urdf/box.urdf")
        self.kdl_kin = KDLKinematics(robot, 'base_link', 'human')

        self.sub = None
        self.timer = None
        self.vel_control = rospy.get_param('~vel_control', True)
        rospy.set_param("~vel_control", self.vel_control)
        self.rate = rate
        self.current = copy(self.start_pos)
        self.pub = rospy.Publisher('robot/joint_states',
                                   JointState,
                                   queue_size=100)
        self.srv = rospy.Service('~switch', Empty, self.switch)
        self.left_endpoint_pub = rospy.Publisher(
            '/robot/limb/left/endpoint_state', EndpointState, queue_size=100)
        self.right_endpoint_pub = rospy.Publisher(
            '/robot/limb/right/endpoint_state', EndpointState, queue_size=100)
        self.box_sub = rospy.Subscriber('box/human_grip', PoseStamped,
                                        self.update_box_joints)
        self.tf_buffer = tf2_ros.Buffer()
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
        self.endpoint_sub = rospy.Subscriber('tf', TFMessage,
                                             self.publish_endpoints)
        if self.vel_control:
            self.vel_init()
        else:
            self.pos_init()

        rospy.loginfo("Initialization complete.")
예제 #7
0
    def __init__(self, urdf_path, sem_path, name_suffix=None):
        self.nsmap = {
            "xsd": "http://www.w3.org/2001/XMLSchema#",
            "owl": "http://www.w3.org/2002/07/owl#",
            "xsd": "http://www.w3.org/2001/XMLSchema#",
            "srdl2": "http://knowrob.org/kb/srdl2.owl#",
            "owl2xml": "http://www.w3.org/2006/12/owl2-xml#",
            "knowrob": "http://knowrob.org/kb/knowrob.owl#",
            "rdfs": "http://www.w3.org/2000/01/rdf-schema#",
            "rdf": "http://www.w3.org/1999/02/22-rdf-syntax-ns#",
            "srdl2-comp": "http://knowrob.org/kb/srdl2-comp.owl#",
            "srdl2-cap": "http://knowrob.org/kb/srdl2-cap.owl#",
            "qudt-unit": "http://qudt.org/vocab/unit#",
        }
        self.imports = [
            "package://srdl/owl/srdl2-comp.owl",
            "package://knowrob_common/owl/knowrob.owl",
        ]
        self.id_gen = UniqueStringGenerator()
        self.urdf = URDF.from_xml_file(urdf_path)
        self.urdf.check_valid()

        basename = os.path.basename(sem_path)
        namespace, _ = os.path.splitext(basename)
        self.map_ns = namespace
        if name_suffix is None:
            self.map_name = self.urdf.name + "_" + self.id_gen.gen()
        else:
            self.map_name = self.urdf.name + "_" + name_suffix
        self.map_uri_base = "http://knowrob.org/kb/%s" % basename
        self.map_uri = self.map_uri_base + "#"
        self.nsmap[self.map_ns] = self.map_uri
        self.transformations = {}
 def __init__(self, urdf, base_link='base_link', endpoint='human', initial_alpha=None):
     self.robot = URDF.from_xml_file(urdf)
     self.kdl_kin = KDLKinematics(self.robot, base_link, endpoint)
     num_joints = self.kdl_kin.num_joints
     super(BoxOptimizer, self).__init__(num_joints, self.kdl_kin.jacobian, self.kdl_kin.jacobian, 'joint_states',
                                        np.zeros(num_joints), np.random.random(3),initial_alpha,
                                        self.kdl_kin.random_joint_angles)
예제 #9
0
    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
예제 #10
0
def JS_to_PrPlRrl(q):

    robot = URDF.from_xml_file(
        "/home/josh/catkin_ws/src/baxter_common/baxter_description/urdf/baxter.urdf"
    )

    kdl_kin_left = KDLKinematics(robot, "base", "left_gripper")
    kdl_kin_right = KDLKinematics(robot, "base", "right_gripper")

    T_left = kdl_kin_left.forward(q[0:7])
    R_left = np.array(T_left[:3, :3])

    T_right = kdl_kin_right.forward(q[7:14])
    R_right = np.array(T_right[:3, :3])

    x_left = T_left[0, 3]
    y_left = T_left[1, 3]
    z_left = T_left[2, 3]

    x_right = T_right[0, 3]
    y_right = T_right[1, 3]
    z_right = T_right[2, 3]

    R_rl = np.dot(np.transpose(R_right), R_left)

    roll, pitch, yaw = euler_from_matrix(R_rl, 'sxyz')

    P = np.array([[x_left], [y_left], [z_left], [x_right], [y_right],
                  [z_right], [roll], [pitch], [yaw], [R_right[0, 0]],
                  [R_left[0, 1]], [R_right[1, 0]], [R_left[1, 1]]])

    return P
예제 #11
0
    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())
예제 #12
0
    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
예제 #13
0
    def __init__(self):
        # moveit_commander.roscpp_initialize(sys.argv)
        # self.arm = moveit_commander.MoveGroupCommander('manipulator_ur5')
        rospy.init_node('moveit_fk_demo', anonymous=True)
        rospy.Subscriber('/joint_states',
                         JointState,
                         self.obtain_joint_states_sim,
                         queue_size=1)

        self.robot = URDF.from_xml_file(
            "/home/zy/catkin_ws/src/universal_robot/ur_description/urdf/ur5.urdf"
        )
        self.aubo_q = [pi, -pi / 2, pi / 2, pi, -pi / 2, 0]
        self.aubo_q1 = np.matrix(self.aubo_q)
        self.kp = 50
        self.Kr = 1
        self.r_dsr = np.matrix([-0.392, -0.109, 0.609])
        self.rdot_dsr = np.matrix([0.0, 0.0, 0.0])
        self.rate = rospy.Rate(10)  # 10hz
        self.urscript_pub = rospy.Publisher("/ur_driver/URScript",
                                            String,
                                            queue_size=10)
        self.error_r_pub = rospy.Publisher("/polishingrobot/error_r",
                                           Vector3,
                                           queue_size=10)
        self.t = 0
        self.vel = 1.05
        self.ace = 1.4
예제 #14
0
 def test_model_eq(self):
     urdf_model = URDF.from_xml_file(res_pkg_path('package://kineverse/urdf/testbot.urdf'))
     ks1 = ArticulationModel()
     ks2 = ArticulationModel()
     load_urdf(ks1, Path(urdf_model.name), urdf_model)
     load_urdf(ks2, Path(urdf_model.name), urdf_model)
     self.assertEquals(ks1, ks2)
예제 #15
0
    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())
예제 #16
0
def main(urdf_file, bag_file, out_file):
    robot = URDF.from_xml_file(urdf_file)
    kdl_kin = KDLKinematics(robot, BASE_LINK, END_LINK)
    joint_names = kdl_kin.get_joint_names()
    link_names = kdl_kin.get_link_names()

    csv_headers = ['time'] + [
        link + axis
        for link, axis in itertools.product(link_names, AXIS_SUFFIX)
    ]

    with open(out_file, 'wb') as f:
        writer = csv.DictWriter(f,
                                fieldnames=csv_headers,
                                quoting=csv.QUOTE_MINIMAL)
        writer.writeheader()

        bag = rosbag.Bag(bag_file)
        for topic, msg, t in bag.read_messages(topics=['/joint_states']):
            joint_vals = {
                name: val
                for name, val in zip(msg.name, msg.position)
            }
            q = [joint_vals[n] for n in joint_names]

            vals = {'time': t.secs + 1e-9 * t.nsecs}
            for i, link in enumerate(link_names):
                pos = kdl_kin._do_kdl_fk(q, i)[0:3, 3].ravel().tolist()[0]
                vals.update({
                    link + axis: pos[val]
                    for val, axis in enumerate(AXIS_SUFFIX)
                })

            writer.writerow(vals)
예제 #17
0
 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
예제 #18
0
def make_joint_limit_dic_from_urdf_file(file):
    robot = URDF.from_xml_file(file)
    dic = {}

    for joint in robot.joints:
        #add joints to dictionary dic with key=joint.name, value=joint.limit
        dic[joint.name] = joint.limit
    return dic
예제 #19
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 ja(q):
    n = mat([0]*36,dtype = float).reshape(6,6)
    robot = URDF.from_xml_file("/home/xsm/control/src/learning_communication/src/ur5_hole.urdf")
    tree = kdl_tree_from_urdf_model(robot)
    chain = tree.getChain("base", "tool0")
    kdl_kin = KDLKinematics(robot, "base_link", "ee_link", tree)
    pose = kdl_kin.jacobian(q) # forward kinematics (returns homogeneous 4x4 numpy.mat)
    return pose
예제 #21
0
 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 load_urdf(self, path):
     self.path = path
     self.robot = URDF.from_xml_file(path)
     self.links = self.parse_links(self.robot)
     self.joints = self.parse_joints(self.robot)
     self.link_names = self.parse_link_names(self.links)
     self.joint_names = self.parse_joint_names(self.joints)
     self.base = self.robot.get_root()
     self.end_links = self.get_end_links()
     self.foot_links, self.foot_links_ns = self.get_foot_links()
예제 #23
0
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()
예제 #24
0
    def __init__(self, base_link='bottom_plate', end_link='camera_link', \
            path="/media/karmesh/DATA/CMU/Courses/Robot_Autonomy/Homework/hw3_release/hw3_release/code/locobot_description/urdf/locobot_description.urdf"):

        self.axis = None
        self.position = None
        self.camera_link_to_camera_depth_frame = self.createTransformationMatrix(
            [0, 0, 0], [0, 0.0149, 0])
        robot = URDF.from_xml_file(path)
        self.urdfParser(robot, base_link, end_link)
        self.loadURDFinfo()
        self.extrinsics_H = None
예제 #25
0
 def get_jacabian_from_joint(self,urdfname):
     robot = URDF.from_xml_file(urdfname)
     # print("hello")
     kdl_kin = KDLKinematics(robot, "base_link", "tool0")
     # print("ok")
     jointq=self.ur_reader.now_ur_pos
     J = kdl_kin.jacobian(jointq)
     print("jointq is:",jointq)
     pose = kdl_kin.forward(jointq)   
     print("pose is:",pose)
     return J,pose
예제 #26
0
def readXacroFile(description_file):
    # xml_robot = URDF.from_parameter_server()
    urdf_file = '/tmp/description.urdf'
    print('Parsing description file ' + description_file)
    execute('xacro ' + description_file + ' -o ' + urdf_file, verbose=True)  # create a temp urdf file
    try:
        xml_robot = URDF.from_xml_file(urdf_file)  # read teh urdf file
    except:
        raise ValueError('Could not parse description file ' + description_file)

    return xml_robot
예제 #27
0
    def test_double_reload(self):
        km = ArticulationModel()
        urdf_model = URDF.from_xml_file(res_pkg_path('package://kineverse/urdf/testbot.urdf'))
        load_urdf(km, Path(urdf_model.name), urdf_model)
        km.clean_structure()
        eef_frame_1 = km.get_data(Path(urdf_model.name) + Path('links/gripper_link/pose'))

        load_urdf(km, Path(urdf_model.name), urdf_model)
        km.clean_structure()
        eef_frame_2 = km.get_data(Path(urdf_model.name) + Path('links/gripper_link/pose'))

        self.assertEquals(eef_frame_1, eef_frame_2)
예제 #28
0
    def __init__(self):
        # moveit_commander.roscpp_initialize(sys.argv)
        # self.arm = moveit_commander.MoveGroupCommander('manipulator_ur5')

        rospy.init_node('moveit_fk_demo', anonymous=True)
        rospy.Subscriber('/joint_states',
                         JointState,
                         self.obtain_joint_states_sim,
                         queue_size=1)
        self.robot = URDF.from_xml_file(
            "/home/zy/catkin_ws/src/universal_robot/ur_description/urdf/ur5.urdf"
        )
        self.urscript_pub = rospy.Publisher("/ur_driver/URScript",
                                            String,
                                            queue_size=10)
        self.error_r_pub = rospy.Publisher("/polishingrobot/error_r",
                                           Vector3,
                                           queue_size=10)
        self.rdes_pub = rospy.Publisher("/polishingrobot/rdes",
                                        Vector3,
                                        queue_size=10)
        self.r_pub = rospy.Publisher("/polishingrobot/r",
                                     Vector3,
                                     queue_size=10)
        self.condition_num_pub = rospy.Publisher(
            "/polishingrobot/condition_num", Float64, queue_size=10)
        self.rank_pub = rospy.Publisher("/polishingrobot/rank",
                                        Int64,
                                        queue_size=10)

        self.q4_pub = rospy.Publisher("/polishingrobot/q4",
                                      Float64,
                                      queue_size=10)
        self.q6_pub = rospy.Publisher("/polishingrobot/q6",
                                      Float64,
                                      queue_size=10)

        self.aubo_q = [
            1.50040841e-03, -2.83640237e+00, 1.53798406e+00, 1.29841831e+00,
            1.50040840e-03, 3.14159265e+00
        ]
        # self.aubo_q=[0.4169788306196942, -1.3621199297826534, -2.011464437502717, -2.22014083451496, -1.5707963267948966, 1.1538174961752024]
        self.aubo_q1 = np.matrix(self.aubo_q)
        self.rate = rospy.Rate(10)  # 10hz
        self.vel = 1.05
        self.ace = 1.4
        self.t = 0

        self.kpr = 10
        self.kq4 = 1000
        self.kq6 = 1000
        self.bq4 = pi / 20
        self.bq6 = pi / 20
    def compute_forward_kinematics():
 #          robot_urdf = URDF.from_xml_string("<robot name='reemc'></robot>") NON FUNZIONA!
        print("forward kinematics")
        robot_urdf = URDF.from_parameter_server()

        print(robot_urdf)
        robot_urdf = URDF.from_xml_file("reemc_simulation/reemc_gazebo/models/reemc_full/model.urdf")
        kdl_kin = KDLKinematics(robot_urdf, 'arm_right_1_joint', 'arm_right_7_joint')
        tree = kdl_tree_from_urdf_model(robot_urdf)
        print (tree.getNrOfSegments())
        chain = tree.getChain( 'arm_right_1_joint', 'arm_right_7_joint')
        print (chain.getNrOfJoints())
    def __init__(self, command_line_arguments):
        file_paths = self.process_input(command_line_arguments)
        for fp in file_paths:

            robot = URDF.from_xml_file(fp)

            if check_compatibility(robot):
                p = convert(robot)
                self.write_params_to_yaml(p, fp)
            else:
                print(robot.name + " is not compatibel with opw_kinematics.")
                continue
예제 #31
0
 def __init__(self):
     rospack = RosPack()
     urdf_file = join(rospack.get_path('human_moveit_config'), 'urdf',
                      'human.urdf')
     model = URDF.from_xml_file(urdf_file)
     self.model_dict = {}
     for j in model.joints:
         self.model_dict[j.child] = {}
         self.model_dict[j.child]['parent_joint'] = {
             'name': j.name,
             'type': j.type
         }
         self.model_dict[j.child]['parent_link'] = j.parent
예제 #32
0
    def __init__(self, base_link='bottom_plate', end_link='ar_tag', \
            path="/media/karmesh/DATA/CMU/Courses/Robot_Autonomy/Homework/hw3_release/hw3_release/code/locobot_description/urdf/locobot_description.urdf"):

        self.axis = None
        self.position = None
        arm_cuboid_properties = np.load('../data/arm_cuboid_properties.npz')
        self.arm_cuboid_origin = arm_cuboid_properties[
            arm_cuboid_properties.files[0]]
        self.dimension = arm_cuboid_properties[arm_cuboid_properties.files[1]]
        print(self.dimension)
        robot = URDF.from_xml_file(path)
        self.urdfParser(robot, base_link, end_link)
        self.loadURDFinfo()
예제 #33
0
def runPatch(input_file, patch_yaml, output_file):
    input_robot = URDF.from_xml_file(input_file)
    patch_param = yaml.load(open(patch_yaml))
    for joint_name in patch_param.keys():
        diff = patch_param[joint_name]
        if diff.has_key("xyz"):
            j = input_robot.joint_map[joint_name]
            j.origin.xyz = diff["xyz"]
        if diff.has_key("rpy"):
            j = input_robot.joint_map[joint_name]
            j.origin.rpy = diff["rpy"]
    if output_file:
        with open(output_file, "w") as f:
            f.write(input_robot.to_xml_string())
    else:
        print input_robot.to_xml_string()
예제 #34
0
import rospy
import rospkg
from urdf_parser_py.urdf import URDF
from tf_cache import LocalTfCache
import std_redirect

#ROS Global Config stuff

#get configuration constants from the param server
#this class recursively parses configuration attributes into properties, letting us use the easy . syntax
class _ParamNode:
    def __init__(self, **entries): 
        self.__dict__.update(entries)
        for key in self.__dict__:
        	if type(self.__dict__[key]) is dict:
        		self.__dict__[key] = _ParamNode(**self.__dict__[key])

def config(prefix = ""):
	return _ParamNode(**rospy.get_param(prefix))

import geometry

with std_redirect.SysRedirect(stderr=std_redirect.devnull): #squelch annoying URDF warnings
	try:
		urdf = URDF.from_parameter_server()
	except KeyError:
		urdf = URDF.from_xml_file(rospkg.RosPack().get_path('spacejockey') + '/resources/spacejockey.urdf')

import kinematics