def __init__(self, urdf, base_link, end_link, kdl_tree=None):
if kdl_tree is None:
kdl_tree = kdl_tree_from_urdf_model(urdf)
self.tree = kdl_tree
self.urdf = urdf
base_link = base_link.split("/")[-1] # for dealing with tf convention
end_link = end_link.split("/")[-1] # for dealing with tf convention
self.chain = kdl_tree.getChain(base_link, end_link)
self.base_link = base_link
self.end_link = end_link
# record joint information in easy-to-use lists
self.joint_limits_lower = []
self.joint_limits_upper = []
self.joint_safety_lower = []
self.joint_safety_upper = []
self.joint_types = []
for jnt_name in self.get_joint_names():
jnt = urdf.joint_map[jnt_name]
if jnt.limit is not None:
self.joint_limits_lower.append(jnt.limit.lower)
self.joint_limits_upper.append(jnt.limit.upper)
else:
self.joint_limits_lower.append(None)
self.joint_limits_upper.append(None)
if jnt.limit is not None:
self.joint_safety_lower.append(jnt.limit.lower)
self.joint_safety_upper.append(jnt.limit.upper)
else:
self.joint_safety_lower.append(None)
self.joint_safety_upper.append(None)
self.joint_types.append(jnt.joint_type)
def replace_none(x, v):
if x is None:
return v
return x
self.joint_limits_lower = np.array([replace_none(jl, -np.inf)
for jl in self.joint_limits_lower])
self.joint_limits_upper = np.array([replace_none(jl, np.inf)
for jl in self.joint_limits_upper])
# reverse these arrays
self.joint_safety_lower = np.array([replace_none(jl, -np.inf)
for jl in self.joint_safety_lower])[::-1]
self.joint_safety_upper = np.array([replace_none(jl, np.inf)
for jl in self.joint_safety_upper])[::-1]
self.joint_types = np.array(self.joint_types)
self.num_joints = len(self.get_joint_names())
self._fk_kdl = kdl.ChainFkSolverPos_recursive(self.chain)
self._ik_v_kdl = kdl.ChainIkSolverVel_pinv(self.chain)
mins_kdl = joint_list_to_kdl(self.joint_safety_lower)
maxs_kdl = joint_list_to_kdl(self.joint_safety_upper)
self._ik_p_kdl = kdl.ChainIkSolverPos_NR_JL(self.chain, mins_kdl, maxs_kdl,
self._fk_kdl, self._ik_v_kdl)
self._jac_kdl = kdl.ChainJntToJacSolver(self.chain)
self._dyn_kdl = kdl.ChainDynParam(self.chain, kdl.Vector().Zero())
def kinem_chain(self, name_frame_end, name_frame_base='odom'):
# Transform URDF to Chain() for the joints between 'name_frame_end' and 'name_frame_base'
self.chain = kdl.Chain()
ik_lambda = 0.35
try:
self.joint_names = self.urdf_model.get_chain(name_frame_base,
name_frame_end,
links=False,
fixed=False)
self.name_frame_in = name_frame_base
self.name_frame_out = name_frame_end
# rospy.loginfo("Will control the following joints: %s" %(self.joint_names))
self.kdl_tree = kdl_tree_from_urdf_model(self.urdf_model)
self.chain = self.kdl_tree.getChain(name_frame_base,
name_frame_end)
self.kdl_fk_solver = kdl.ChainFkSolverPos_recursive(self.chain)
self.kdl_ikv_solver = kdl.ChainIkSolverVel_wdls(self.chain)
self.kdl_ikv_solver.setLambda(ik_lambda)
self.nJoints = self.chain.getNrOfJoints()
# Default Task and Joint weights
self.tweights = np.identity(6)
# weight matrix with 1 in diagonal to make use of all the joints.
self.jweights = np.identity(self.nJoints)
self.kdl_ikv_solver.setWeightTS(self.tweights.tolist())
self.kdl_ikv_solver.setWeightJS(self.jweights.tolist())
# Fill the list with the joint limits
self.joint_limits_lower = np.empty(self.nJoints)
self.joint_limits_upper = np.empty(self.nJoints)
self.joint_vel_limits = np.empty(self.nJoints)
for n, jnt_name in enumerate(self.joint_names):
jnt = self.urdf_model.joint_map[jnt_name]
if jnt.limit is not None:
if jnt.limit.lower is None:
self.joint_limits_lower[n] = -0.07
else:
self.joint_limits_lower[n] = jnt.limit.lower
if jnt.limit.upper is None:
self.joint_limits_upper[n] = -0.07
else:
self.joint_limits_upper[n] = jnt.limit.upper
self.joint_vel_limits[n] = jnt.limit.velocity
self.joint_vel_limits[0] = 0.3
self.joint_vel_limits[1] = 0.3
except (RuntimeError, TypeError, NameError):
rospy.logerr("Unexpected error:", sys.exc_info()[0])
rospy.logerr('Could not re-init the kinematic chain')
self.name_frame_out = ''
def __init__(self, root_link=None, tip_link="left_arm_7_link"):
self._urdf = URDF.from_parameter_server(key='robot_description')
self._kdl_tree = kdl_tree_from_urdf_model(self._urdf)
self._base_link = self._urdf.get_root(
) if root_link is None else root_link
self._tip_link = tip_link
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
self._joint_names = self._urdf.get_chain(self._base_link,
self._tip_link,
links=False,
fixed=False)
self._num_jnts = len(self._joint_names)
# KDL
self._fk_p_kdl = PyKDL.ChainFkSolverPos_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)
# trac_ik
urdf_str = rospy.get_param('/robot_description')
self.trac_ik_solver = IK(self._base_link,
self._tip_link,
urdf_string=urdf_str)
self.joint_limits_lower = []
self.joint_limits_upper = []
self.joint_types = []
for jnt_name in self._joint_names:
jnt = self._urdf.joint_map[jnt_name]
if jnt.limit is not None:
self.joint_limits_lower.append(jnt.limit.lower)
self.joint_limits_upper.append(jnt.limit.upper)
else:
self.joint_limits_lower.append(None)
self.joint_limits_upper.append(None)
self.joint_types.append(jnt.type)
self._default_seed = [
-0.3723750412464142, 0.02747996523976326, -0.7221865057945251,
-1.69843590259552, 0.11076358705759048, 0.5450965166091919,
0.45358774065971375
] # home_pos
def initialize_kinematic_solvers(self):
robot_description = rospy.get_param('/yumi/velocity_control/robot_description', '/robot_description')
self._robot = URDF.from_parameter_server(key=robot_description)
self._kdl_tree = kdl_tree_from_urdf_model(self._robot)
# self._base_link = self._robot.get_root()
self._ee_link = 'gripper_' +self._arm_name + '_base' #name of the frame we want
self._ee_frame = PyKDL.Frame()
self._ee_arm_chain = self._kdl_tree.getChain(self._base_link, self._ee_link)
# KDL Solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._ee_arm_chain)
self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._ee_arm_chain)
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._ee_arm_chain)
#self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_LMA(self._ee_arm_chain)
self._jac_kdl = PyKDL.ChainJntToJacSolver(self._ee_arm_chain)
self._dyn_kdl = PyKDL.ChainDynParam(self._ee_arm_chain, PyKDL.Vector.Zero())
def nizhuan(q):
RR = array([[0,1,0],[1,0,0],[0,0,-1]])
n= mat([[0.0,1.0,0.0],[1.0,0.0,0.0],[0.0,0.0,-1.0]])
print(n)
robot = URDF.from_xml_file("/home/xsm/control/src/learning_communication/src/ur5_hole.urdf")
tree = kdl_tree_from_urdf_model(robot)
kdl_kin = KDLKinematics(robot, "base_link", "ee_link", tree)
pose = kdl_kin.forward(q) # forward kinematics (returns homogeneous 4x4 numpy.mat)
pose[2,1] = -pose[2,1]
pose[2,2] = -pose[2,2]
pose[0,0] = -pose[0,0]
pose[1,0] = -pose[1,0]
n[0:3,0] = pose[0:3,1]
n[0:3,1] = pose[0:3,2]
n[0:3,2] = pose[0:3,0]
n = linalg.pinv(dot(RR,n))
return array(n)
def kinem_chain(self,
name_frame_end,
name_frame_base='triangle_base_link'):
# Transform URDF to Chain() for the joints between 'name_frame_end' and 'name_frame_base'
self.chain = kdl.Chain()
try:
self.joint_names = self.urdf_model.get_chain(name_frame_base,
name_frame_end,
links=False,
fixed=False)
self.name_frame_in = name_frame_base
self.name_frame_out = name_frame_end
self.njoints = len(self.joint_names)
self.kdl_tree = kdl_tree_from_urdf_model(self.urdf_model)
self.chain = self.kdl_tree.getChain(name_frame_base,
name_frame_end)
self.kdl_fk_solver = kdl.ChainFkSolverPos_recursive(self.chain)
self.kdl_ikv_solver = kdl.ChainIkSolverVel_wdls(self.chain)
self.kdl_ikv_solver.setLambda(self.ik_lambda)
# Default Task and Joint weights
self.tweights = np.identity(6)
# weight matrix with 1 in diagonal to make use of all the joints.
self.jweights = np.identity(self.njoints)
self.kdl_ikv_solver.setWeightTS(self.tweights.tolist())
self.kdl_ikv_solver.setWeightJS(self.jweights.tolist())
# Fill the list with the joint limits
self.joint_limits_lower = []
self.joint_limits_upper = []
for jnt_name in self.joint_names:
jnt = self.urdf_model.joint_map[jnt_name]
if jnt.limit is not None:
self.joint_limits_lower.append(jnt.limit.lower)
self.joint_limits_upper.append(jnt.limit.upper)
self.nJoints = self.chain.getNrOfJoints()
except:
rospy.logerr("Unexpected error:", sys.exc_info()[0])
rospy.logerr('Could not re-init the kinematic chain')
self.name_frame_out = ''
return self.chain
def zheng(q):
n = mat([0]*36,dtype = float).reshape(6,6)
robot = URDF.from_xml_file("/home/xsm/control/src/learning_communication/src/ur5_robot.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.forward(q) # forward kinematics (returns homogeneous 4x4 numpy.mat)
posee = deepcopy(pose)
pose[2,1] = -pose[2,1]
pose[2,2] = -pose[2,2]
pose[0,0] = -pose[0,0]
pose[1,0] = -pose[1,0]
posee[:,2] = pose[:,0]
posee[:,1] = pose[:,2]
posee[:,0] = pose[:,1]
n[0:3,0] = pose[0:3,1]
n[0:3,1] = pose[0:3,2]
n[0:3,2] = pose[0:3,0]
n[3:6,3] = pose[0:3,1]
n[3:6,4] = pose[0:3,2]
n[3:6,5] = pose[0:3,0]
return array(n)
def __init__(self, limb):
self._baxter = URDF.from_parameter_server(key='robot_description')
self._kdl_tree = kdl_tree_from_urdf_model(self._baxter)
self._base_link = self._baxter.get_root()
self._tip_link = limb + '_gripper'
self._tip_frame = PyKDL.Frame()
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
# Baxter Interface Limb Instances
self._limb_interface = baxter_interface.Limb(limb)
self._joint_names = self._limb_interface.joint_names()
self._num_jnts = len(self._joint_names)
# KDL Solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain)
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain,
self._fk_p_kdl,
self._ik_v_kdl)
self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain)
self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain,
PyKDL.Vector.Zero())
def __init__(self, urdf, base_link, end_link, kdl_tree=None):
if kdl_tree is None:
kdl_tree = kdl_tree_from_urdf_model(urdf)
self.tree = kdl_tree
self.urdf = urdf
base_link = base_link.split("/")[-1] # for dealing with tf convention
end_link = end_link.split("/")[-1] # for dealing with tf convention
self.chain = kdl_tree.getChain(base_link, end_link)
self.base_link = base_link
self.end_link = end_link
# record joint information in easy-to-use lists
self.joint_limits_lower = []
self.joint_limits_upper = []
self.joint_safety_lower = []
self.joint_safety_upper = []
self.joint_types = []
for jnt_name in self.get_joint_names():
jnt = urdf.joint_map[jnt_name]
if jnt.limit is not None:
self.joint_limits_lower.append(jnt.limit.lower)
self.joint_limits_upper.append(jnt.limit.upper)
else:
self.joint_limits_lower.append(None)
self.joint_limits_upper.append(None)
if jnt.safety_controller is not None:
self.joint_safety_lower.append(
jnt.safety_controller.soft_lower_limit) #.lower)
self.joint_safety_upper.append(
jnt.safety_controller.soft_upper_limit) #.upper)
elif jnt.limit is not None:
self.joint_safety_lower.append(jnt.limit.lower)
self.joint_safety_upper.append(jnt.limit.upper)
else:
self.joint_safety_lower.append(None)
self.joint_safety_upper.append(None)
self.joint_types.append(jnt.type)
def replace_none(x, v):
if x is None:
return v
return x
self.joint_limits_lower = np.array(
[replace_none(jl, -np.inf) for jl in self.joint_limits_lower])
self.joint_limits_upper = np.array(
[replace_none(jl, np.inf) for jl in self.joint_limits_upper])
self.joint_safety_lower = np.array(
[replace_none(jl, -np.inf) for jl in self.joint_safety_lower])
self.joint_safety_upper = np.array(
[replace_none(jl, np.inf) for jl in self.joint_safety_upper])
self.joint_types = np.array(self.joint_types)
self.num_joints = len(self.get_joint_names())
self._fk_kdl = kdl.ChainFkSolverPos_recursive(self.chain)
self._ik_v_kdl = kdl.ChainIkSolverVel_pinv(self.chain)
self._ik_p_kdl = kdl.ChainIkSolverPos_NR(self.chain, self._fk_kdl,
self._ik_v_kdl)
self._jac_kdl = kdl.ChainJntToJacSolver(self.chain)
self._dyn_kdl = kdl.ChainDynParam(self.chain, kdl.Vector.Zero())
def __init__(self, urdf, start, end):
r = URDF.from_xml_string(hacky_urdf_parser_fix(urdf))
tree = kdl_tree_from_urdf_model(r)
self.chain = tree.getChain(start, end)
self.fksolver = PyKDL.ChainFkSolverPos_recursive(self.chain)
