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 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.dh_params = [[-0.033, pi / 2, 0.145, pi, -1],
[0.155, 0, 0, pi / 2, -1],
[0.135, 0, 0, 0.0, -1],
[-0.002, pi / 2, 0, -pi / 2, -1],
[0, pi, -0.185, -pi, -1]]
self.joint_offset = [170*pi/180, 65*pi/180, -146*pi/180, 102.5*pi/180, 167.5*pi/180]
self.joint_limit_min = [-169*pi/180, -65*pi/180, -151*pi/180, -102.5*pi/180, -167.5*pi/180]
self.joint_limit_max = [169*pi/180, 90*pi/180, 146*pi/180, 102.5*pi/180, 167.5*pi/180]
# Setup the subscribers for the joint states
self.subscriber_joint_state_ = rospy.Subscriber('/joint_states', JointState, self.joint_state_callback,
queue_size=5)
# TF2 broadcaster
self.pose_broadcaster = tf2_ros.TransformBroadcaster()
# PyKDL
self.kine_chain = PyKDL.Chain()
self.setup_kdl_chain()
self.current_joint_position = PyKDL.JntArray(self.kine_chain.getNrOfJoints())
self.current_pose = PyKDL.Frame()
self.fk_solver = PyKDL.ChainFkSolverPos_recursive(self.kine_chain)
self.ik_solver = PyKDL.ChainIkSolverPos_LMA(self.kine_chain)
self.jac_calc = PyKDL.ChainJntToJacSolver(self.kine_chain)
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 __init__(self):
# Subscriptions
self.objects = rospy.Subscriber('/found_objects', Object,
self.callback_obj)
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer)
self.gp_action = actionlib.SimpleActionClient('request_trajectory',
RequestTrajectoryAction)
self.grasping_poses = []
self.object_list = []
self.old_list = []
self.joint_limits_lower = []
self.joint_limits_upper = []
self.joint_names = []
self.gp_weights = np.zeros(3)
# Arm selection
self.left_arm = False
self.right_arm = False
self.frame_end = ''
self.desired_chain = 'left_chain'
self.manip_threshold = 0.05
self.distance_threshold = 1.15
# Gripper frames:
self.grip_left = 'left_gripper_tool_frame'
# self.grip_right = 'left_gripper_tool_frame'
self.grip_right = 'right_gripper_tool_frame'
self.frame_base = 'base_link'
# KDL chains:
self.right_chain = kdl.Chain()
self.left_chain = kdl.Chain()
self.ik_lambda = 0.35 # how much should singularities be avoided?
self.arms_chain() # generate chains for both arms
self.left_jnt_pos = kdl.JntArray(self.nJoints)
self.right_jnt_pos = kdl.JntArray(self.nJoints)
self.jac_solver_left = kdl.ChainJntToJacSolver(self.left_chain)
self.jac_solver_right = kdl.ChainJntToJacSolver(self.right_chain)
self.joint_list = rospy.Subscriber('/joint_states', JointState,
self.joint_callback)
rospy.sleep(0.01)
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 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 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 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 __init__(self):
rospy.init_node('goal_selector', anonymous=True)
r = rospy.Rate(2)
self.objects = rospy.Subscriber('/found_objects', Object, self.callback_obj)
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer)
self.grasping_poses = []
self.object_list = []
self.old_list =[]
self.joint_limits_lower = []
self.joint_limits_upper = []
self.joint_names = []
self.gp_weights = np.zeros(6)
# Arm selection
self.left_arm = False
self.right_arm = False
self.frame_end = ''
self.desired_chain = 'left_chain'
self.manip_threshold = 0.05
self.distance_threshold = 1.15
# Gripper frames:
self.grip_left = 'left_gripper_tool_frame'
self.grip_right = 'right_gripper_tool_frame'
self.frame_base = 'base_footprint'
# KDL chains:
self.right_chain = kdl.Chain()
self.left_chain = kdl.Chain()
self.ik_lambda = 0.35 # how much should singularities be avoided?
self.arms_chain() # generate chains for both arms
self.left_jnt_pos = kdl.JntArray(self.nJoints)
self.right_jnt_pos = kdl.JntArray(self.nJoints)
self.jac_solver_left = kdl.ChainJntToJacSolver(self.left_chain)
self.jac_solver_right = kdl.ChainJntToJacSolver(self.right_chain)
self.joint_list = rospy.Subscriber('/joint_states', JointState, self.joint_callback)
rospy.sleep(0.5)
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):
super(YoubotKDL, self).__init__()
# PyKDL
self.kine_chain = PyKDL.Chain()
self.setup_kdl_chain()
self.current_joint_position = PyKDL.JntArray(
self.kine_chain.getNrOfJoints())
self.current_pose = PyKDL.Frame()
self.fk_solver = PyKDL.ChainFkSolverPos_recursive(self.kine_chain)
self.ik_solver = PyKDL.ChainIkSolverPos_LMA(self.kine_chain)
self.jac_calc = PyKDL.ChainJntToJacSolver(self.kine_chain)
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 _set_solvers(self):
"""Initialize the solvers according to the stored base chain and
tool"""
self._complete_chain = kdl.Chain(self._base_chain)
if self._tool_segment is not None:
if isinstance(self._tool_segment, kdl.Segment):
self._complete_chain.addSegment(self._tool_segment)
else:
raise Exception("Tool is not None, Chain or Segment")
self._fk_solver_pos = kdl.ChainFkSolverPos_recursive(
self._complete_chain)
self._tv_solver = kdl.ChainIkSolverVel_pinv(self._complete_chain)
self._ik_solver_pos = kdl.ChainIkSolverPos_NR(self._complete_chain,
self._fk_solver_pos,
self._tv_solver)
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 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 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 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))
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)
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
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)
#! /usr/bin/python
import rospy
import json
import PyKDL as kdl
import os
from sensor_msgs.msg import *
from geometry_msgs.msg import *
from tf.transformations import *
from visualization_msgs.msg import Marker
kdlChain = kdl.Chain()
frame = kdl.Frame()
print("przed")
print(kdlChain)
print("po")
with open(os.path.dirname(os.path.realpath(__file__)) + '/../dh.json',
'r') as file:
params = json.loads(file.read())
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()):
#!/usr/bin/env python
import rospy
from math import pi
import numpy as np
from sensor_msgs.msg import JointState
from geometry_msgs.msg import TransformStamped, Quaternion
import tf2_ros
import PyKDL as kdl
chain = kdl.Chain()
a = [0.0, 0.265699, 0.03, 0.0, 0.0, 0.0]
alpha = [-pi / 2, 0.0, -pi / 2, -pi / 2, -pi / 2, 0.0]
d = [0.159, 0.0, 0.0, 0.258, 0.0, -0.123]
theta = [
0.0, -pi / 2 + np.arctan(0.03 / 0.264), -np.arctan(0.03 / 0.264), 0.0, 0.0,
0.0
]
name_link = [
'kdl_link_1', 'kdl_link_2', 'kdl_link_3', 'kdl_link_4', 'kdl_link_5',
'kdl_link_6'
]
chain = kdl.Chain()
for p_a, p_alpha, p_d, p_theta in zip(a, alpha, d, theta):
chain.addSegment(
kdl.Segment(kdl.Joint(kdl.Joint.RotZ),
kdl.Frame().DH(a=p_a, alpha=p_alpha, d=p_d,
theta=p_theta)))
def callback(data):
kdl_chain =PyKDL.Chain()
Frame = PyKDL.Frame();
i = 0
d=0
th=0
for joint in dh_file:
name = joint['name']
last_d = d
last_th = th
a = joint["a"]
d = joint["d"]
al = joint["al"]
th = joint["th"]
if name == 'i3' and get_parameters('dlugosc1'):
a = rospy.get_param("/dlugosc1")
if name == 'hand' and get_parameters('dlugosc2'):
a = rospy.get_param("/dlugosc2")
#forego first iteration
if i==0:
i = i+1
continue
kdl_chain.addSegment(PyKDL.Segment(PyKDL.Joint(PyKDL.Joint.RotZ), Frame.DH(a, al, last_d, last_th)))
i = i+1
jointDisplacement = PyKDL.JntArray(kdl_chain.getNrOfJoints())
#joint displacements including restrictions
jointDisplacement[0] = data.position[0]
jointDisplacement[1] = data.position[1]
jointDisplacement[2] = data.position[2]
if(data.position[0] < restrictions[0]['backward']):
jointDisplacement[0] = restrictions[0]['backward']
rospy.logerr("[KDL] Przekroczono ograniczenie dolne stawu o numerze: 1")
elif(data.position[0] > restrictions[0]['forward']):
jointDisplacement[0] = restrictions[0]['forward']
rospy.logerr("[KDL] Przekroczono ograniczenie gorne stawu o numerze: 1")
if(data.position[1] < restrictions[1]['backward']):
jointDisplacement[1] = restrictions[1]['backward']
rospy.logerr("[KDL] Przekroczono ograniczenie dolne stawu o numerze: 2")
elif(data.position[1] > restrictions[1]['forward']):
jointDisplacement[1] = restrictions[1]['forward']
rospy.logerr("[KDL] Przekroczono ograniczenie gorne stawu o numerze: 2")
if(data.position[2] < restrictions[2]['backward']):
jointDisplacement[2] = restrictions[2]['backward']
rospy.logerr("[KDL] Przekroczono ograniczenie dolne stawu o numerze: 3")
elif(data.position[2] > restrictions[2]['forward']):
jointDisplacement[2] = restrictions[2]['forward']
rospy.logerr("[KDL] Przekroczono ograniczenie gorne stawu o numerze: 3")
f_k_solver = PyKDL.ChainFkSolverPos_recursive(kdl_chain)
frame = PyKDL.Frame()
f_k_solver.JntToCart(jointDisplacement, frame)
quatr = frame.M.GetQuaternion()
kdl_pose = PoseStamped()
kdl_pose.header.frame_id = 'base_link'
kdl_pose.header.stamp = rospy.Time.now()
kdl_pose.pose.position.x = frame.p[0]
kdl_pose.pose.position.y = frame.p[1]
kdl_pose.pose.position.z = frame.p[2]
kdl_pose.pose.orientation.x = quatr[0]
kdl_pose.pose.orientation.y = quatr[1]
kdl_pose.pose.orientation.z = quatr[2]
kdl_pose.pose.orientation.w = quatr[3]
pub.publish(kdl_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
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
