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
