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))
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)
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
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)
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.")
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)
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 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
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 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):
# 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
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)
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 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)
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 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
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
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()
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, 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
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
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
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)
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
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
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()
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()
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