def _add_children_to_tree(robot_model, root, tree):
# constructs the optional inertia
inert = kdl.RigidBodyInertia(0)
if root.inertial:
inert = _toKdlInertia(root.inertial)
# constructs the kdl joint
(parent_joint_name, parent_link_name) = robot_model.parent_map[root.name]
parent_joint = robot_model.joint_map[parent_joint_name]
# construct the kdl segment
sgm = kdl.Segment(root.name, _toKdlJoint(parent_joint),
_toKdlPose(parent_joint.origin), inert)
# add segment to tree
if not tree.addSegment(sgm, parent_link_name):
return False
if root.name not in robot_model.child_map:
return True
children = [
robot_model.link_map[l] for (j, l) in robot_model.child_map[root.name]
]
# recurslively add all children
for child in children:
if not _add_children_to_tree(robot_model, child, tree):
return False
return True
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 add_children_to_tree(parent, segment_id):
if parent in urdf.child_map:
for joint, child_name in urdf.child_map[parent]:
if joint in js_inactive_names_vector:
print "setting as fixed:", joint, js_pos[joint]
joint_rot = -js_pos[joint]
urdf.joint_map[joint].joint_type = 'fixed'
else:
joint_rot = 0.0
child = urdf.link_map[child_name]
if child.inertial is not None:
kdl_inert = kdl_urdf.urdf_inertial_to_kdl_rbi(
child.inertial)
else:
kdl_inert = PyKDL.RigidBodyInertia()
kdl_jnt = kdl_urdf.urdf_joint_to_kdl_joint(
urdf.joint_map[joint])
kdl_origin = kdl_urdf.urdf_pose_to_kdl_frame(
urdf.joint_map[joint].origin) * PyKDL.Frame(
PyKDL.Rotation.RotZ(joint_rot))
kdl_sgm = PyKDL.Segment(child_name, kdl_jnt, kdl_origin,
kdl_inert)
segment_map[segment_id] = kdl_sgm
segment_parent_map[segment_id] = segment_name_id_map[
parent]
segment_name_id_map[child_name] = segment_id
segment_id += 1
tree.addSegment(kdl_sgm, parent)
segment_id = add_children_to_tree(child_name, segment_id)
return segment_id
def urdf_inertial_to_kdl_rbi(i):
origin = urdf_pose_to_kdl_frame(i.origin)
rbi = kdl.RigidBodyInertia(
i.mass, origin.p,
kdl.RotationalInertia(i.inertia.ixx, i.inertia.iyy, i.inertia.izz,
i.inertia.ixy, i.inertia.ixz, i.inertia.iyz))
return origin.M * rbi
def add_children_to_tree(parent):
if parent in urdf.child_map:
if len(urdf.child_map[parent]) > 1:
ef_tree[parent] = [
child_name for _, child_name in urdf.child_map[parent]
]
for joint, child_name in urdf.child_map[parent]:
for lidx, link in enumerate(urdf.links):
if child_name == link.name:
child = urdf.links[lidx]
if child.inertial is not None:
kdl_inert = urdf_inertial_to_kdl_rbi(
child.inertial)
else:
kdl_inert = kdl.RigidBodyInertia()
for jidx, jnt in enumerate(urdf.joints):
if jnt.name == joint:
kdl_jnt = urdf_joint_to_kdl_joint(
urdf.joints[jidx])
kdl_origin = urdf_pose_to_kdl_frame(
urdf.joints[jidx].origin)
kdl_sgm = kdl.Segment(child_name, kdl_jnt,
kdl_origin, kdl_inert)
tree.addSegment(kdl_sgm, parent)
add_children_to_tree(child_name)
def _toKdlInertia(i):
# kdl specifies the inertia in the reference frame of the link, the urdf
# specifies the inertia in the inertia reference frame
origin = _toKdlPose(i.origin)
inertia = i.inertia
return origin.M * PyKDL.RigidBodyInertia(
i.mass, origin.p,
PyKDL.RotationalInertia(inertia.ixx, inertia.iyy, inertia.izz, inertia.ixy, inertia.ixz, inertia.iyz))
def urdf_inertial_to_kdl_rbi(i):
origin = urdf_pose_to_kdl_frame(i.origin)
rbi = kdl.RigidBodyInertia(i.mass, origin.p,
kdl.RotationalInertia(i.matrix['ixx'],
i.matrix['iyy'],
i.matrix['izz'],
i.matrix['ixy'],
i.matrix['ixz'],
i.matrix['iyz']))
return origin.M * rbi
def add_children_to_tree(parent):
if parent in urdf.child_map:
for joint, child_name in urdf.child_map[parent]:
child = urdf.link_map[child_name]
if child.inertial is not None:
kdl_inert = urdf_inertial_to_kdl_rbi(child.inertial)
else:
kdl_inert = kdl.RigidBodyInertia()
kdl_jnt = urdf_joint_to_kdl_joint(urdf.joint_map[joint])
kdl_origin = urdf_pose_to_kdl_frame(urdf.joint_map[joint].origin)
kdl_sgm = kdl.Segment(child_name, kdl_jnt,
kdl_origin, kdl_inert)
tree.addSegment(kdl_sgm, parent)
add_children_to_tree(child_name)
def __init__(self,
limb,
ee_frame_name="panda_link8",
additional_segment_config=None,
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)
if additional_segment_config is not None:
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_link = ee_frame_name
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 add_children_to_tree(root, tree):
children = root.child_links
inert = kdl.RigidBodyInertia()
if root.inertial is not None:
inert = to_kdl_rbi(root.inertial)
jnt = to_kdl_joint(root.parent_joint)
sgm = kdl.Segment(
root.name, jnt,
to_kdl_frame(root.parent_joint.parent_to_joint_origin_transform),
inert)
tree.addSegment(sgm, root.parent_joint.parent_link_name)
for child in children:
add_children_to_tree(child, tree)
def addChildrenToTree(robotModel, root, tree):
"""
Helper function that adds children to a KDL tree.
"""
# constructs the optional inertia
inert = kdl.RigidBodyInertia(0)
if root.inertial:
inert = toKdlInertia(root.inertial)
# constructs the kdl joint
parentJointName, parentLinkName = robotModel.parent_map[root.name]
parentJoint = robotModel.joint_map[parentJointName]
# construct the kdl segment
sgm = kdl.Segment(
root.name,
toKdlJoint(parentJoint),
toKdlPose(parentJoint.origin),
inert)
# add segment to tree
if not tree.addSegment(sgm, parentLinkName):
return False
if root.name not in robotModel.child_map:
return True
children = [robotModel.link_map[l] for (j, l) in robotModel.child_map[root.name]]
# recurslively add all children
for child in children:
if not addChildrenToTree(robotModel, child, tree):
return False
return True
def to_kdl_rbi(i):
origin = to_kdl_frame(i.origin)
rbi = kdl.RigidBodyInertia(
i.mass, origin.p,
kdl.RotationalInertia(i.ixx, i.iyy, i.ixy, i.ixz, i.iyz))
return rbi.rot_mult(origin.M)
import PyKDL
#********创建段*******#
#坐标
frame1 = PyKDL.Frame(PyKDL.Rotation.RPY(0, 1, 0), PyKDL.Vector(3, 2, 4))
print "frame1:\n", frame1
#关节
joint1 = PyKDL.Joint(PyKDL.Joint.RotZ)
print "joint:", joint1
#惯性张量
m = 0.5
oc = PyKDL.Vector(1, 0, 1)
Ic = PyKDL.RotationalInertia(1.0, 2.0, 3.0, 4.0, 5.0, 6.0)
RBI = PyKDL.RigidBodyInertia(m, oc, Ic)
m = RBI.getMass()
print "m:\n", m
oc = RBI.getCOG()
print "oc:\n", oc
#创建段
segment1 = PyKDL.Segment(joint1, frame1, RBI)
print "segment1:\n", segment1
#********创建链*******#
links = []
for i in range(6):
links.append(segment1)
def __init__(self, T=20, weight=[1.0, 1.0], verbose=True):
#initialize model
self.chain = PyKDL.Chain()
self.chain.addSegment(
PyKDL.Segment(
"Base", PyKDL.Joint(PyKDL.Joint.None),
PyKDL.Frame(PyKDL.Vector(0.0, 0.0, 0.042)),
PyKDL.RigidBodyInertia(
0.08659, PyKDL.Vector(0.00025, 0.0, -0.02420),
PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.chain.addSegment(
PyKDL.Segment(
"Joint1", PyKDL.Joint(PyKDL.Joint.RotZ),
PyKDL.Frame(PyKDL.Vector(0.0, -0.019, 0.028)),
PyKDL.RigidBodyInertia(
0.00795, PyKDL.Vector(0.0, 0.019, -0.02025),
PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.chain.addSegment(
PyKDL.Segment(
"Joint2", PyKDL.Joint(PyKDL.Joint.RotY),
PyKDL.Frame(PyKDL.Vector(0.0, 0.019, 0.0405)),
PyKDL.RigidBodyInertia(
0.09312, PyKDL.Vector(0.00000, -0.00057, -0.02731),
PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.chain.addSegment(
PyKDL.Segment(
"Joint3", PyKDL.Joint(PyKDL.Joint.RotZ),
PyKDL.Frame(PyKDL.Vector(0.024, -0.019, 0.064)),
PyKDL.RigidBodyInertia(
0.19398, PyKDL.Vector(-0.02376, 0.01864, -0.02267),
PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.chain.addSegment(
PyKDL.Segment(
"Joint4",
PyKDL.Joint("minus_RotY", PyKDL.Vector(0, 0, 0),
PyKDL.Vector(0, -1, 0), PyKDL.Joint.RotAxis),
PyKDL.Frame(PyKDL.Vector(0.064, 0.019, 0.024)),
PyKDL.RigidBodyInertia(
0.09824, PyKDL.Vector(-0.02099, 0.0, -0.01213),
PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.chain.addSegment(
PyKDL.Segment(
"Joint5", PyKDL.Joint(PyKDL.Joint.RotX),
PyKDL.Frame(PyKDL.Vector(0.0405, -0.019, 0.0)),
PyKDL.RigidBodyInertia(
0.09312, PyKDL.Vector(-0.01319, 0.01843, 0.0),
PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.chain.addSegment(
PyKDL.Segment(
"Joint6",
PyKDL.Joint("minus_RotY", PyKDL.Vector(0, 0, 0),
PyKDL.Vector(0, -1, 0), PyKDL.Joint.RotAxis),
PyKDL.Frame(PyKDL.Vector(0.064, 0.019, 0.0)),
PyKDL.RigidBodyInertia(
0.09824, PyKDL.Vector(-0.02099, 0.0, 0.01142),
PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.chain.addSegment(
PyKDL.Segment(
"Joint7", PyKDL.Joint(PyKDL.Joint.RotX),
PyKDL.Frame(PyKDL.Vector(0.14103, 0.0, 0.0)),
PyKDL.RigidBodyInertia(
0.26121, PyKDL.Vector(-0.09906, 0.00146, -0.00021),
PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
self.min_position_limit = [
-160.0, -90.0, -160.0, -90.0, -160.0, -90.0, -160.0
]
self.max_position_limit = [
160.0, 90.0, 160.0, 90.0, 160.0, 90.0, 160.0
]
self.min_joint_position_limit = PyKDL.JntArray(7)
self.max_joint_position_limit = PyKDL.JntArray(7)
for i in range(0, 7):
self.min_joint_position_limit[
i] = self.min_position_limit[i] * pi / 180
self.max_joint_position_limit[
i] = self.max_position_limit[i] * pi / 180
self.fksolver = PyKDL.ChainFkSolverPos_recursive(self.chain)
self.iksolver = PyKDL.ChainIkSolverVel_pinv(self.chain)
self.iksolverpos = PyKDL.ChainIkSolverPos_NR_JL(
self.chain, self.min_joint_position_limit,
self.max_joint_position_limit, self.fksolver, self.iksolver, 100,
1e-6)
#parameter
self.T = T
self.weight_u = weight[0]
self.weight_x = weight[1]
self.verbose = verbose
self.nj = self.chain.getNrOfJoints()
self.q_init = np.zeros(self.nj)
self.q_out = np.zeros(self.nj)
ret, self.dest, self.q_out = self.generate_dest()
self.fin_position = self.dest.p
return
