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 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 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 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 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 __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(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 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 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 __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 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 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 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 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 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 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 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 __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))
frm6 = kdl.Frame(kdl.Rotation.RotX(0), kdl.Vector(-168, 0, 0))
frms.append(frm1)
frms.append(frm2)
frms.append(frm3)
frms.append(frm4)
frms.append(frm5)
frms.append(frm6)
print "frames:\n", frms
rbt = kdl.Chain() # 建立机器人对象
#建立连杆
link = []
for i in range(6):
link.append(kdl.Segment(jnts[i], frms[i]))
rbt.addSegment(link[i])
fk = kdl.ChainFkSolverPos_recursive(rbt)
p = kdl.Frame()
q = kdl.JntArray(6)
q[0] = 0
q[1] = 0
q[2] = 0
q[3] = 0
q[4] = 0
q[5] = 0
fk.JntToCart(q, p)
def forward_kinematics(data):
chain = PyKDL.Chain()
frame = PyKDL.Frame()
k = 1
prev_d = 0
prev_th = 0
n_joints = len(params.keys())
for i in params.keys():
a, d, alpha, th = params[i]
alpha, a, d, th = float(alpha), float(a), float(d), float(th)
joint = PyKDL.Joint(PyKDL.Joint.TransZ)
if k != 1:
fr = frame.DH(a, alpha, prev_d, prev_th)
segment = PyKDL.Segment(joint, fr)
chain.addSegment(segment)
k = k + 1
prev_d = d
prev_th = th
a, d, alpha, th = params["i3"]
chain.addSegment(PyKDL.Segment(joint, frame.DH(0, 0, d, th)))
joints = PyKDL.JntArray(n_joints)
for i in range(n_joints):
min_joint, max_joint = scope["joint" + str(i + 1)]
if min_joint <= data.position[i] <= max_joint:
joints[i] = data.position[i]
else:
rospy.logwarn("Wrong joint value")
return
fk = PyKDL.ChainFkSolverPos_recursive(chain)
finalFrame = PyKDL.Frame()
fk.JntToCart(joints, finalFrame)
quaterions = finalFrame.M.GetQuaternion()
pose = PoseStamped()
pose.header.frame_id = 'base_link'
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = finalFrame.p[0]
pose.pose.position.y = finalFrame.p[1]
pose.pose.position.z = finalFrame.p[2]
pose.pose.orientation.x = quaterions[0]
pose.pose.orientation.y = quaterions[1]
pose.pose.orientation.z = quaterions[2]
pose.pose.orientation.w = quaterions[3]
pub.publish(pose)
marker = Marker()
marker.header.frame_id = 'base_link'
marker.type = marker.CUBE
marker.action = marker.ADD
marker.pose.orientation.w = 1
time = rospy.Duration()
marker.lifetime = time
marker.scale.x = 0.09
marker.scale.y = 0.09
marker.scale.z = 0.09
marker.pose.position.x = finalFrame.p[0]
marker.pose.position.y = finalFrame.p[1]
marker.pose.position.z = finalFrame.p[2]
marker.pose.orientation.x = quaterions[0]
marker.pose.orientation.y = quaterions[1]
marker.pose.orientation.z = quaterions[2]
marker.pose.orientation.w = quaterions[3]
marker.color.a = 0.7
marker.color.r = 0.2
marker.color.g = 0.8
marker.color.b = 0.6
marker_pub.publish(marker)
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
