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 _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 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)
#-*-coding:utf-8-*- #研究运动树和运动学链 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 = []
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
