def callback(data):
rospy.loginfo(rospy.get_caller_id() + 'I heard position %f from %s', data.position[0], data.name[0])
chain = kdl.Chain()
rot = kdl.Rotation(m.cos(th3), m.sin(th3), 0, -1*m.sin(th3)*m.cos(al3), m.cos(th3)*m.cos(al3), m.sin(al3), m.sin(th3)*m.sin(al3), -1*m.cos(th3)*m.sin(al3), m.cos(al3))
chain.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.TransZ), kdl.Frame(rot, kdl.Vector(a3,0.0,d3))))
rot = kdl.Rotation(m.cos(th2), m.sin(th2), 0, -1*m.sin(th2)*m.cos(al2), m.cos(th2)*m.cos(al2), m.sin(al2), m.sin(th2)*m.sin(al2), -1*m.cos(th2)*m.sin(al2), m.cos(al2))
chain.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.TransZ), kdl.Frame(rot, kdl.Vector(a2,0.0,d2))))
rot = kdl.Rotation(m.cos(th1), m.sin(th1), 0, -1*m.sin(th1)*m.cos(al1), m.cos(th1)*m.cos(al1), m.sin(al1), m.sin(th1)*m.sin(al1), -1*m.cos(th1)*m.sin(al1), m.cos(al1))
chain.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.TransZ), kdl.Frame(rot, kdl.Vector(a1,0.0,d1))))
rot = kdl.Rotation(1,0,0, 0,1,0, 0,0,1)
chain.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.None), kdl.Frame(rot, kdl.Vector(0.0,0.0,0.0))))
fksolver = kdl.ChainFkSolverPos_recursive(chain)
jpos = kdl.JntArray(chain.getNrOfJoints())
jpos[2] = data.position[0]
jpos[1] = data.position[1]
jpos[0] = data.position[2]
cartpos = kdl.Frame()
fksolver.JntToCart(jpos,cartpos)
pub_msg = PoseStamped()
pub_msg.header.frame_id = "base_link"
pub_msg.pose.position.x = cartpos.p[2]
pub_msg.pose.position.y =-cartpos.p[1]
pub_msg.pose.position.z = cartpos.p[0]
pub.publish(pub_msg)
def __init__(self):
self.leg = kdl.Chain()
self.leg.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY), kdl.Frame(kdl.Vector(0,0,-50))))
self.leg.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY), kdl.Frame(kdl.Vector(0,0,-50))))
self.ik_solver = kdl.ChainIkSolverPos_LMA(self.leg)
self.alpha1=45
self.alpha2=-45
self.beta=self.alpha2-self.alpha1
def simple_kinematic(data):
'''
realizacja kinematyki prostej z uzyciem biblioteki KDL
wraz z nadaniem obliczonych wartosci
'''
if not find_position(data):
rospy.logerr('Wrong position: ' +
str(data)) #spr czy nie bledna pozycja
return
kdl_chain = kdl.Chain()
kdl_frame = kdl.Frame()
frame0 = kdl_frame.DH(0, 0, 0, 0)
joint0 = kdl.Joint(kdl.Joint.None)
kdl_chain.addSegment(kdl.Segment(joint0, frame0))
a, d, alfa, theta = params['i2']
frame1 = kdl_frame.DH(a, alfa, d, theta)
joint1 = kdl.Joint(kdl.Joint.RotZ)
kdl_chain.addSegment(kdl.Segment(joint1, frame1))
a, d, alfa, theta = params['i1']
frame2 = kdl_frame.DH(a, alfa, d, theta)
joint2 = kdl.Joint(kdl.Joint.RotZ)
kdl_chain.addSegment(kdl.Segment(joint2, frame2))
a, d, alfa, theta = params['i3']
frame3 = kdl_frame.DH(a, alfa, d, theta)
joint3 = kdl.Joint(kdl.Joint.TransZ)
kdl_chain.addSegment(kdl.Segment(joint3, frame3))
joint_angles = kdl.JntArray(kdl_chain.getNrOfJoints())
joint_angles[0] = data.position[0]
joint_angles[1] = data.position[1]
joint_angles[2] = -data.position[2]
#kinematyka prosta przeliczenie na translacje i rotacje czlonu koncowego
kdl_chain_t = kdl.ChainFkSolverPos_recursive(kdl_chain)
frame_t = kdl.Frame()
kdl_chain_t.JntToCart(joint_angles, frame_t)
quat = frame_t.M.GetQuaternion()
pose = PoseStamped()
pose.header.frame_id = 'base_link'
pose.header.stamp = rospy.Time.now()
#nadanie wiadomosci z wyliczonymi wspolrzednymi czlonu koncowego
pose.pose.position.x = frame_t.p[0]
pose.pose.position.y = frame_t.p[1]
pose.pose.position.z = frame_t.p[2]
pose.pose.orientation.x = quat[0]
pose.pose.orientation.y = quat[1]
pose.pose.orientation.z = quat[2]
pose.pose.orientation.w = quat[3]
pub.publish(pose)
def simpleKinematics(data):
#jesli polozenie jest niepoprawne zostaje wypisana informacja
if not checkPosition(data):
rospy.logerr('Wrong position: ' + str(data))
return
#utworzenie trzech kolejnych macierzy z parametrow D-H
kdl_chain = kdl.Chain()
kdl_frame = kdl.Frame()
frame0 = kdl_frame.DH(0, 0, 0, 0)
joint0 = kdl.Joint(kdl.Joint.None)
kdl_chain.addSegment(kdl.Segment(joint0, frame0))
a, d, alpha, theta = params['i2']
frame1 = kdl_frame.DH(a, alpha, d, theta)
joint1 = kdl.Joint(kdl.Joint.RotZ)
kdl_chain.addSegment(kdl.Segment(joint1, frame1))
a, d, alpha, theta = params['i1']
frame2 = kdl_frame.DH(a, alpha, d, theta)
joint2 = kdl.Joint(kdl.Joint.RotZ)
kdl_chain.addSegment(kdl.Segment(joint2, frame2))
a, d, alpha, theta = params['i3']
frame3 = kdl_frame.DH(a, alpha, d, theta)
joint3 = kdl.Joint(kdl.Joint.TransZ)
kdl_chain.addSegment(kdl.Segment(joint3, frame3))
#przeliczenie poprzednich macierzy na finalne wartosci koncowki
joints_angle = kdl.JntArray(kdl_chain.getNrOfJoints())
joints_angle[0] = data.position[0]
joints_angle[1] = data.position[1]
joints_angle[2] = -data.position[2]
kdl_chain_solver = kdl.ChainFkSolverPos_recursive(kdl_chain)
final_frame = kdl.Frame()
kdl_chain_solver.JntToCart(joints_angle, final_frame)
quaternion = final_frame.M.GetQuaternion()
#utworzenie i nadanie wiadomosci z polozeniem koncowki
pose = PoseStamped()
pose.header.frame_id = 'base'
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = final_frame.p[0]
pose.pose.position.y = final_frame.p[1]
pose.pose.position.z = final_frame.p[2]
pose.pose.orientation.x = quaternion[0]
pose.pose.orientation.y = quaternion[1]
pose.pose.orientation.z = quaternion[2]
pose.pose.orientation.w = quaternion[3]
pub.publish(pose)
def __init__(self):
self.arm = kdl.Chain()
self.arm.addSegment(
kdl.Segment(kdl.Joint(kdl.Joint.RotZ),
kdl.Frame(kdl.Vector(300, 0, 0))))
self.arm.addSegment(
kdl.Segment(kdl.Joint(kdl.Joint.RotZ),
kdl.Frame(kdl.Vector(0, -200, 0))))
self.ik_solver = kdl.ChainIkSolverPos_LMA(self.arm)
self.current_joints = kdl.JntArray(self.arm.getNrOfJoints())
self.result_joints = kdl.JntArray(self.arm.getNrOfJoints())
def callback(data):
kdlChain = kdl.Chain()
frame = kdl.Frame()
with open(
os.path.dirname(os.path.realpath(__file__)) + '/../dh.json',
'r') as file:
params = json.loads(file.read())
matrices = {}
for key in sorted(params.keys()):
a, d, al, th = params[key]
if key == '1':
d_prev = d
th_prev = th
continue
print(params[key])
al, a, d, th = float(al), float(a), float(d), float(th)
kdlChain.addSegment(
kdl.Segment(kdl.Joint(kdl.Joint.RotZ),
frame.DH(a, al, d_prev, th_prev)))
d_prev = d
th_prev = th
kdlChain.addSegment(
kdl.Segment(kdl.Joint(kdl.Joint.RotZ), frame.DH(0, 0, d_prev,
th_prev)))
jointPos = kdl.JntArray(kdlChain.getNrOfJoints())
jointPos[0] = data.position[0]
jointPos[1] = data.position[1]
jointPos[2] = data.position[2]
forvKin = kdl.ChainFkSolverPos_recursive(kdlChain)
eeFrame = kdl.Frame()
forvKin.JntToCart(jointPos, eeFrame)
quaternion = eeFrame.M.GetQuaternion()
robot_pose = PoseStamped()
robot_pose.header.frame_id = 'base_link'
robot_pose.header.stamp = rospy.Time.now()
robot_pose.pose.position.x = eeFrame.p[0]
robot_pose.pose.position.y = eeFrame.p[1]
robot_pose.pose.position.z = eeFrame.p[2]
robot_pose.pose.orientation.x = quaternion[0]
robot_pose.pose.orientation.y = quaternion[1]
robot_pose.pose.orientation.z = quaternion[2]
robot_pose.pose.orientation.w = quaternion[3]
publisher.publish(robot_pose)
def __init__(self, linear_transform):
"""Initialize the Universal Robot class using its dh parmeters."""
linear_frame = m3d_transform_to_kdl_frame(linear_transform)
chain = kdl.Chain()
# Add linear segment
chain.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.TransX),
linear_frame))
# Add UR segments
for segment in range(self.NUM_JOINTS):
joint = kdl.Joint(kdl.Joint.RotZ)
frame = kdl.Frame().DH(self.DH_A[segment], self.DH_ALPHA[segment],
self.DH_D[segment], 0)
chain.addSegment(kdl.Segment(joint, frame))
Kinematics.__init__(self, chain)
def urdf_joint_to_kdl_joint(jnt):
origin_frame = urdf_pose_to_kdl_frame(jnt.origin)
if jnt.joint_type == 'fixed':
return kdl.Joint(jnt.name, kdl.Joint.Fixed)
axis = kdl.Vector(*jnt.axis)
if jnt.joint_type == 'revolute':
return kdl.Joint(jnt.name, origin_frame.p, origin_frame.M * axis,
kdl.Joint.RotAxis)
if jnt.joint_type == 'continuous':
return kdl.Joint(jnt.name, origin_frame.p, origin_frame.M * axis,
kdl.Joint.RotAxis)
if jnt.joint_type == 'prismatic':
return kdl.Joint(jnt.name, origin_frame.p, origin_frame.M * axis,
kdl.Joint.TransAxis)
print("Unknown joint type: %s." % jnt.joint_type)
def setup_kdl_chain(self):
for dh in self.dh_params:
self.kine_chain.addSegment(
PyKDL.Segment(
PyKDL.Joint(PyKDL.Vector(), PyKDL.Vector(0, 0, dh[4]),
PyKDL.Joint.RotAxis),
PyKDL.Frame().DH(dh[0], dh[1], dh[2], dh[3])))
def urdf_joint_to_kdl_joint(jnt):
origin_frame = urdf_pose_to_kdl_frame(jnt.origin)
if jnt.joint_type == 'fixed':
return kdl.Joint(jnt.name, getattr(kdl.Joint, 'None'))
axis = kdl.Vector(*[float(s) for s in jnt.axis])
if jnt.joint_type == 'revolute':
return kdl.Joint(jnt.name, origin_frame.p, origin_frame.M * axis,
kdl.Joint.RotAxis)
if jnt.joint_type == 'continuous':
return kdl.Joint(jnt.name, origin_frame.p, origin_frame.M * axis,
kdl.Joint.RotAxis)
if jnt.joint_type == 'prismatic':
return kdl.Joint(jnt.name, origin_frame.p, origin_frame.M * axis,
kdl.Joint.TransAxis)
print("Unknown joint type: %s." % jnt.joint_type)
return kdl.Joint(jnt.name, getattr(kdl.Joint, 'None'))
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 _toKdlJoint(jnt):
fixed = lambda j,F: PyKDL.Joint(j.name, getattr(PyKDL.Joint, 'None'))
rotational = lambda j,F: PyKDL.Joint(j.name, F.p, F.M * PyKDL.Vector(*j.axis), PyKDL.Joint.RotAxis)
translational = lambda j,F: PyKDL.Joint(j.name, F.p, F.M * PyKDL.Vector(*j.axis), PyKDL.Joint.TransAxis)
type_map = {
'fixed': fixed,
'revolute': rotational,
'continuous': rotational,
'prismatic': translational,
'floating': fixed,
'planar': fixed,
'unknown': fixed,
}
return type_map[jnt.type](jnt, _toKdlPose(jnt.origin))
def create_right_chain(self):
ch = kdl.Chain()
self.right_arm_base_offset_from_torso_lift_link = np.matrix(
[0., -0.188, 0.]).T
# shoulder pan
ch.addSegment(
kdl.Segment(kdl.Joint(kdl.Joint.RotZ),
kdl.Frame(kdl.Vector(0.1, 0., 0.))))
# shoulder lift
ch.addSegment(
kdl.Segment(kdl.Joint(kdl.Joint.RotY),
kdl.Frame(kdl.Vector(0., 0., 0.))))
# upper arm roll
ch.addSegment(
kdl.Segment(kdl.Joint(kdl.Joint.RotX),
kdl.Frame(kdl.Vector(0.4, 0., 0.))))
# elbox flex
ch.addSegment(
kdl.Segment(kdl.Joint(kdl.Joint.RotY),
kdl.Frame(kdl.Vector(0.0, 0., 0.))))
# forearm roll
ch.addSegment(
kdl.Segment(kdl.Joint(kdl.Joint.RotX),
kdl.Frame(kdl.Vector(0.321, 0., 0.))))
# wrist flex
ch.addSegment(
kdl.Segment(kdl.Joint(kdl.Joint.RotY),
kdl.Frame(kdl.Vector(0., 0., 0.))))
# wrist roll
ch.addSegment(
kdl.Segment(kdl.Joint(kdl.Joint.RotX),
kdl.Frame(kdl.Vector(0., 0., 0.))))
return ch
def toKdlJoint(jnt):
# define a mapping for joints and kdl
fixed = lambda j, F: kdl.Joint(j.name)
rotational = lambda j, F: kdl.Joint(j.name, F.p, F.M * kdl.Vector(*j.axis), kdl.Joint.RotAxis)
translational = lambda j, F: kdl.Joint(j.name, F.p, F.M * kdl.Vector(*j.axis),
kdl.Joint.TransAxis)
typeMap = {
'fixed': fixed,
'revolute': rotational,
'continuous': rotational,
'prismatic': translational,
'floating': fixed,
'planar': fixed,
'unknown': fixed,
}
return typeMap[jnt.type](jnt, toKdlPose(jnt.origin))
def __init__(self):
"""Initialize the Universal Robot class using its dh parmeters."""
chain = kdl.Chain()
for segment in range(self.NUM_JOINTS):
joint = kdl.Joint(kdl.Joint.RotZ)
frame = kdl.Frame().DH(self.DH_A[segment], self.DH_ALPHA[segment],
self.DH_D[segment], 0)
chain.addSegment(kdl.Segment(joint, frame))
Kinematics.__init__(self, chain)
def to_kdl_joint(jnt):
f_parent_jnt = to_kdl_frame(jnt.parent_to_joint_origin_transform)
if jnt.type == urdf.Joint.FIXED:
return kdl.Joint(jnt.name, kdl.Joint.None)
elif jnt.type == urdf.Joint.REVOLUTE:
axis = to_kdl_vector(jnt.axis)
return kdl.Joint(jnt.name, f_parent_jnt.p, f_parent_jnt.M * axis,
kdl.Joint.RotAxis)
elif jnt.type == urdf.Joint.CONTINUOUS:
axis = to_kdl_vector(jnt.axis)
return kdl.Joint(jnt.name, f_parent_jnt.p, f_parent_jnt.M * axis,
kdl.Joint.RotAxis)
elif jnt.type == urdf.Joint.PRISMATIC:
axis = to_kdl_vector(jnt.axis)
return kdl.Joint(jnt.name, f_parent_jnt.p, f_parent_jnt.M * axis,
kdl.Joint.RotAxis)
else:
rospy.logwarn("Unknown joint type %s." % jnt.name)
return kdl.Joint(jnt.name, kdl.Joint.None)
def create_right_chain(self, end_effector_length):
ch = kdl.Chain()
ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,-0.18465,0.))))
ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotX),kdl.Frame(kdl.Vector(0.,-0.03175,0.))))
ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotZ),kdl.Frame(kdl.Vector(0.00502,0.,-0.27857))))
ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,0.,-0.27747))))
ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotZ),kdl.Frame(kdl.Vector(0.,0.,0.))))
ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,0.,0.))))
ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotX),kdl.Frame(kdl.Vector(0.,0.,-end_effector_length))))
return ch
def create_left_chain(self, end_effector_length):
ch = kdl.Chain()
ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,0.18493,0.))))
ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotX),kdl.Frame(kdl.Vector(0.,0.03175,0.))))
ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotZ),kdl.Frame(kdl.Vector(0.00635,0.,-0.27795))))
ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,0.,-0.27853))))
ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotZ),kdl.Frame(kdl.Vector(0.,0.,0.))))
ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,0.,0.))))
ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotX),kdl.Frame(kdl.Vector(0.,0.,-end_effector_length))))
return ch
def set_tool_transform(self, tool=None):
"""Set a tool transformation
Keyword Arguments
tool -- The tool as either None, Segment, Frame or
math3d Transform (default None)
"""
if tool is None:
self._tool_segment = None
elif isinstance(tool, kdl.Segment):
self._tool_segment = kdl.Segment(tool)
elif isinstance(tool, kdl.Frame):
self._tool_segment = kdl.Segment(kdl.Joint(), tool)
elif isinstance(tool, m3d.Transform):
frame = m3d_transform_to_kdl_frame(tool)
self._tool_segment = kdl.Segment(kdl.Joint(), frame)
else:
raise Exception(
"Tool is not None, Segment, Frame or m3d Transform")
self._set_solvers()
return True
def urdf_joint_to_kdl_joint(jnt):
origin_frame = urdf_pose_to_kdl_frame(jnt.origin)
try: # kdl issue to run with python 2
fixed_joint = kdl.Joint.Fixed
except:
joint_type_str = "kdl.Joint.None"
fixed_joint = eval(joint_type_str)
if jnt.joint_type == "fixed":
return kdl.Joint(jnt.name, fixed_joint) # None
axis = kdl.Vector(*[float(s) for s in jnt.axis])
if jnt.joint_type == "revolute":
return kdl.Joint(jnt.name, origin_frame.p, origin_frame.M * axis,
kdl.Joint.RotAxis)
if jnt.joint_type == "continuous":
return kdl.Joint(jnt.name, origin_frame.p, origin_frame.M * axis,
kdl.Joint.RotAxis)
if jnt.joint_type == "prismatic":
return kdl.Joint(jnt.name, origin_frame.p, origin_frame.M * axis,
kdl.Joint.TransAxis)
print("Unknown joint type: %s." % jnt.joint_type)
return kdl.Joint(jnt.name, fixed_joint)
def kin_fwd(params):
publish = True
if not check(params):
publish = False
rospy.logerr('Impossible position: ' + str(params))
return
k = 1
chain = pykdl.Chain()
frame = pykdl.Frame()
angles = pykdl.JntArray(3)
prev_d = 0
prev_theta = 0
counter = 0
for i in parametres.keys():
a, d, alpha, theta = parametres[i]
a, d, alpha, theta = float(a), float(d), float(alpha), float(theta)
joint = pykdl.Joint(pykdl.Joint.TransZ)
if k != 1:
fr = frame.DH(a, alpha, prev_d, prev_theta)
chain.addSegment(pykdl.Segment(joint, fr))
k = k + 1
prev_d = d
prev_theta = theta
chain.addSegment(pykdl.Segment(joint, frame.DH(0, 0, d, theta)))
angles[0] = params.position[0]
angles[1] = params.position[1]
angles[2] = params.position[2]
solver = pykdl.ChainFkSolverPos_recursive(chain)
secFrame = pykdl.Frame()
solver.JntToCart(angles, secFrame)
quater = secFrame.M.GetQuaternion()
pose = PoseStamped()
pose.header.frame_id = 'base_link'
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = secFrame.p[0]
pose.pose.position.y = secFrame.p[1]
pose.pose.position.z = secFrame.p[2]
pose.pose.orientation.x = quater[0]
pose.pose.orientation.y = quater[1]
pose.pose.orientation.z = quater[2]
pose.pose.orientation.w = quater[3]
if publish:
publisher.publish(pose)
def __init__(self, DH):
self.DH = DH
##在构造函数中建立机器人模型
#DH(theta,alpha,a,d)
n = len(DH[:, 0])
#建立机器人坐标，关节
frms = []
jnts = []
for i in range(n):
jnts.append(kdl.Joint(kdl.Joint.RotZ))
#建立坐标系
trans = bf.trans(DH[i, 0], DH[i, 1], DH[i, 2], DH[i, 3])
vetor = kdl.Vector(trans[3, 0], trans[3, 1], trans[3, 2])
frm1 = kdl.Frame(kdl.Rotation.RotX(-np.pi / 2),
kdl.Vector(0, 0, 239.5))
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 forward_kinematics(data):
if not checkScope(data):
publish = False
rospy.logwarn('Incorrect joint position[KDL]!')
return
chain = pykdl.Chain()
frame = pykdl.Frame()
angles = pykdl.JntArray(3)
prev_d = 0
k = 1
prev_theta = 0
counter = 0
for i in params.keys():
a, d, alpha, theta = params[i]
a, d, alpha, theta = float(a), float(d), float(alpha), float(theta)
joint = pykdl.Joint(pykdl.Joint.TransZ)
if k != 1:
fr = frame.DH(a, alpha, prev_d, prev_theta)
chain.addSegment(pykdl.Segment(joint, fr))
k = k + 1
prev_d = d
prev_theta = theta
chain.addSegment(pykdl.Segment(joint, frame.DH(0, 0, d, theta)))
angles[0] = data.position[0]
angles[1] = data.position[1]
angles[2] = data.position[2]
solver = pykdl.ChainFkSolverPos_recursive(chain)
secFrame = pykdl.Frame()
solver.JntToCart(angles, secFrame)
quater = secFrame.M.GetQuaternion()
pose = PoseStamped()
pose.header.frame_id = 'base_link'
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = secFrame.p[0]
pose.pose.position.z = secFrame.p[2]
pose.pose.position.y = secFrame.p[1]
pose.pose.orientation.x = quater[0]
pose.pose.orientation.y = quater[1]
pose.pose.orientation.z = quater[2]
pose.pose.orientation.w = quater[3]
pub.publish(pose)
def create_right_chain(self):
height = 0.0
linkage_offset_from_ground = np.matrix([0., 0., height]).T
self.linkage_offset_from_ground = linkage_offset_from_ground
self.n_jts = len(self.d_robot.b_jt_anchor)
rospy.loginfo("number of joints is :"+str(self.n_jts))
ee_location = self.d_robot.ee_location
b_jt_anchor = self.d_robot.b_jt_anchor
b_jt_axis = self.d_robot.b_jt_axis
ch = kdl.Chain()
prev_vec = np.copy(linkage_offset_from_ground.A1)
n = len(self.d_robot.b_jt_anchor)
for i in xrange(n-1):
if b_jt_axis[i][0] == 1 and b_jt_axis[i][1] == 0 and b_jt_axis[i][2] == 0:
kdl_jt = kdl.Joint(kdl.Joint.RotX)
elif b_jt_axis[i][0] == 0 and b_jt_axis[i][1] == 1 and b_jt_axis[i][2] == 0:
kdl_jt = kdl.Joint(kdl.Joint.RotY)
elif b_jt_axis[i][0] == 0 and b_jt_axis[i][1] == 0 and b_jt_axis[i][2] == 1:
kdl_jt = kdl.Joint(kdl.Joint.RotZ)
else:
print "can't do off-axis joints yet!!!"
np_vec = np.array(b_jt_anchor[i+1])
diff_vec = np_vec-prev_vec
prev_vec = np_vec
kdl_vec = kdl.Vector(diff_vec[0], diff_vec[1], diff_vec[2])
ch.addSegment(kdl.Segment(kdl_jt, kdl.Frame(kdl_vec)))
np_vec = np.copy(ee_location.A1)
diff_vec = np_vec-prev_vec
if b_jt_axis[n-1][0] == 1 and b_jt_axis[n-1][1] == 0 and b_jt_axis[n-1][2] == 0:
kdl_jt = kdl.Joint(kdl.Joint.RotX)
elif b_jt_axis[n-1][0] == 0 and b_jt_axis[n-1][1] == 1 and b_jt_axis[n-1][2] == 0:
kdl_jt = kdl.Joint(kdl.Joint.RotY)
elif b_jt_axis[n-1][0] == 0 and b_jt_axis[n-1][1] == 0 and b_jt_axis[n-1][2] == 1:
kdl_jt = kdl.Joint(kdl.Joint.RotZ)
else:
print "can't do off-axis joints yet!!!"
kdl_vec = kdl.Vector(diff_vec[0], diff_vec[1], diff_vec[2])
ch.addSegment(kdl.Segment(kdl_jt, kdl.Frame(kdl_vec)))
return ch
def forward_kinematics(data):
doPub = True
if not correct(data):
rospy.logerr('Incorrect position! ' + str(data))
doPub = False
return
kdlChain = kdl.Chain()
frameFactory = kdl.Frame()
jntAngles = kdl.JntArray(3)
jointNums = [2,3,1]
for i in jointNums:
a, d, al, th = params['i'+str(i)]
al, a, d, th = float(al), float(a), float(d), float(th)
frame = frameFactory.DH(a, al, d, th)
joint = kdl.Joint(kdl.Joint.RotZ)
kdlChain.addSegment(kdl.Segment(joint, frame))
jntAngles[i-1] = data.position[i-1]
fksolver = kdl.ChainFkSolverPos_recursive(kdlChain)
eeFrame = kdl.Frame()
fksolver.JntToCart(jntAngles, eeFrame)
quaternion = eeFrame.M.GetQuaternion()
pose = PoseStamped()
pose.header.frame_id = 'base_link'
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = eeFrame.p[0]
pose.pose.position.y = eeFrame.p[1]
pose.pose.position.z = eeFrame.p[2]
pose.pose.orientation.x = quaternion[0]
pose.pose.orientation.y = quaternion[1]
pose.pose.orientation.z = quaternion[2]
pose.pose.orientation.w = quaternion[3]
if doPub:
pub.publish(pose)
def __init__(self, T=20, weight=[1.0,1.0]):
#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.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
def callback(data):
KDL_chain = PyKDL.Chain()
KDL_frame = PyKDL.Frame()
errorFlag = 0
# base
frame0 = KDL_frame.DH(A[0], Alpha[0], 0, 0)
joint0 = PyKDL.Joint(PyKDL.Joint.None)
KDL_chain.addSegment(PyKDL.Segment(joint0, frame0))
# joint 1
d = rospy.get_param("d1", D[0])
if (data.position[0] < -d) or (data.position[0] > 0):
errorFlag = 1
frame1 = KDL_frame.DH(A[1], Alpha[1], d, Theta[0])
joint1 = PyKDL.Joint(PyKDL.Joint.TransZ)
KDL_chain.addSegment(PyKDL.Segment(joint1, frame1))
# joint 2
d = rospy.get_param("d2", D[1])
if (data.position[1] < -d) or (data.position[1] > 0):
errorFlag = 2
frame2 = KDL_frame.DH(A[2], Alpha[2], d, Theta[1])
joint2 = PyKDL.Joint(PyKDL.Joint.TransZ)
KDL_chain.addSegment(PyKDL.Segment(joint2, frame2))
# joint 3
d = rospy.get_param("d3", D[2])
if (data.position[2] < -d) or (data.position[2] > 0):
errorFlag = 3
frame3 = KDL_frame.DH(0, 0, d, Theta[2])
joint3 = PyKDL.Joint(PyKDL.Joint.TransZ)
KDL_chain.addSegment(PyKDL.Segment(joint3, frame3))
# final touches
joint_array = PyKDL.JntArray(KDL_chain.getNrOfJoints())
joint_array[0] = data.position[0]
joint_array[1] = data.position[1]
joint_array[2] = data.position[2]
KDL_chain_solver = PyKDL.ChainFkSolverPos_recursive(KDL_chain)
frame_end = PyKDL.Frame()
KDL_chain_solver.JntToCart(joint_array, frame_end)
q = frame_end.M.GetQuaternion()
poseStamped = PoseStamped()
poseStamped.header.frame_id = 'base_link'
poseStamped.header.stamp = rospy.Time.now()
poseStamped.pose.position.x = frame_end.p[0]
poseStamped.pose.position.y = frame_end.p[1]
poseStamped.pose.position.z = frame_end.p[2]
poseStamped.pose.orientation.x = q[0]
poseStamped.pose.orientation.y = q[1]
poseStamped.pose.orientation.z = q[2]
poseStamped.pose.orientation.w = q[3]
marker = Marker()
marker.header.frame_id = 'base_link'
marker.header.stamp = rospy.Time.now()
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.pose = poseStamped.pose
marker.scale.x = 0.06
marker.scale.y = 0.06
marker.scale.z = 0.06
marker.color.a = 1
marker.color.r = 1
marker.color.g = 0
marker.color.b = 0
if errorFlag == 0:
pubP.publish(poseStamped)
pubM.publish(marker)
else:
print('Error: joint{} out of bounds'.format(errorFlag))
# coding=utf-8
import numpy as np
import PyKDL as kdl
# import copy #用于深度拷贝
# 注意一些基本概念
# Segment初始化由绕的轴和末端姿态来生成，可能跟书上的D-H方法有点不一样，区别在于用末端和轴来定义一个关节
# 轴是在当前关节的坐标系中绕轴V旋转，默认设置为kdl.Joint.RotZ
# f_tip，末端在当前坐标系中的姿态
#建立关节
jnts = []
for i in range(6):
jnts.append(kdl.Joint(kdl.Joint.RotZ))
print "jnts:\n", jnts
#建立坐标系系列
frms = []
# 注意，这个框架是f_tip，末端
frm1 = kdl.Frame(kdl.Rotation.RotX(-np.pi / 2), kdl.Vector(0, 0, 239.5))
frm2 = kdl.Frame(kdl.Rotation.RotX(0), kdl.Vector(250, 0, 0))
frm3 = kdl.Frame(kdl.Rotation.RotX(np.pi / 2), kdl.Vector(0, 262, 0))
frm4 = kdl.Frame(kdl.Rotation.RotX(-np.pi / 2), kdl.Vector(0, 0, 0))
frm5 = kdl.Frame(kdl.Rotation.RotX(np.pi / 2), kdl.Vector(0, 0, 0))
frm6 = kdl.Frame(kdl.Rotation.RotX(0), kdl.Vector(-168, 0, 0))
frms.append(frm1)
frms.append(frm2)
frms.append(frm3)
def create_right_chain(self):
height = 0.0
linkage_offset_from_ground = np.matrix([0., 0., height]).T
self.linkage_offset_from_ground = linkage_offset_from_ground
b_jt_q_ind = [True, True, True]
self.n_jts = len(b_jt_q_ind)
torso_half_width = 0.196
upper_arm_length = 0.334
forearm_length = 0.288 #+ 0.115
ee_location = np.matrix([
0., -torso_half_width - upper_arm_length - forearm_length, height
]).T
b_jt_anchor = [[0, 0, height], [0, -torso_half_width, height],
[0., -torso_half_width - upper_arm_length, height]]
b_jt_axis = [[0, 0, 1], [0, 0, 1], [0, 0, 1]]
ch = kdl.Chain()
prev_vec = np.copy(linkage_offset_from_ground.A1)
n = len(b_jt_q_ind)
for i in xrange(n - 1):
if b_jt_axis[i][0] == 1 and b_jt_axis[i][1] == 0 and b_jt_axis[i][
2] == 0:
kdl_jt = kdl.Joint(kdl.Joint.RotX)
elif b_jt_axis[i][0] == 0 and b_jt_axis[i][1] == 1 and b_jt_axis[
i][2] == 0:
kdl_jt = kdl.Joint(kdl.Joint.RotY)
elif b_jt_axis[i][0] == 0 and b_jt_axis[i][1] == 0 and b_jt_axis[
i][2] == 1:
kdl_jt = kdl.Joint(kdl.Joint.RotZ)
else:
print "can't do off-axis joints yet!!!"
np_vec = np.array(b_jt_anchor[i + 1])
diff_vec = np_vec - prev_vec
prev_vec = np_vec
kdl_vec = kdl.Vector(diff_vec[0], diff_vec[1], diff_vec[2])
ch.addSegment(kdl.Segment(kdl_jt, kdl.Frame(kdl_vec)))
np_vec = np.copy(ee_location.A1)
diff_vec = np_vec - prev_vec
if b_jt_axis[n - 1][0] == 1 and b_jt_axis[n - 1][1] == 0 and b_jt_axis[
n - 1][2] == 0:
kdl_jt = kdl.Joint(kdl.Joint.RotX)
elif b_jt_axis[n - 1][0] == 0 and b_jt_axis[
n - 1][1] == 1 and b_jt_axis[n - 1][2] == 0:
kdl_jt = kdl.Joint(kdl.Joint.RotY)
elif b_jt_axis[n - 1][0] == 0 and b_jt_axis[
n - 1][1] == 0 and b_jt_axis[n - 1][2] == 1:
kdl_jt = kdl.Joint(kdl.Joint.RotZ)
else:
print "can't do off-axis joints yet!!!"
kdl_vec = kdl.Vector(diff_vec[0], diff_vec[1], diff_vec[2])
ch.addSegment(kdl.Segment(kdl_jt, kdl.Frame(kdl_vec)))
return ch
