def readInPoses(self):
xaplibrary = rospy.get_param('~xap', None)
if xaplibrary:
self.parseXapPoses(xaplibrary)
poses = rospy.get_param('~poses', None)
if poses:
for key, value in poses.iteritems():
try:
# TODO: handle multiple points in trajectory
trajectory = JointTrajectory()
trajectory.joint_names = value["joint_names"]
point = JointTrajectoryPoint()
point.time_from_start = Duration(value["time_from_start"])
point.positions = value["positions"]
trajectory.points = [point]
self.poseLibrary[key] = trajectory
except:
rospy.logwarn(
"Could not parse pose \"%s\" from the param server:",
key)
rospy.logwarn(sys.exc_info())
# add a default crouching pose:
if not "crouch" in self.poseLibrary:
trajectory = JointTrajectory()
trajectory.joint_names = ["Body"]
point = JointTrajectoryPoint()
point.time_from_start = Duration(1.5)
point.positions = [
0.0,
0.0, # head
1.545,
0.33,
-1.57,
-0.486,
0.0,
0.0, # left arm
-0.3,
0.057,
-0.744,
2.192,
-1.122,
-0.035, # left leg
-0.3,
0.057,
-0.744,
2.192,
-1.122,
-0.035, # right leg
1.545,
-0.33,
1.57,
0.486,
0.0,
0.0
] # right arm
trajectory.points = [point]
self.poseLibrary["crouch"] = trajectory
rospy.loginfo("Loaded %d poses: %s", len(self.poseLibrary),
self.poseLibrary.keys())
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
### Your FK code here
# Create symbols
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8') #link offsets
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7') #link lenghts
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols(
'alpha0:7') #twist angles
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
# Create Modified DH parameters
d0_1 = 0.75
d3_4 = 1.5
d6_7 = 0.303
a1_2 = 0.35
a2_3 = 1.25
a3_4 = -0.054
alpha1_2 = -pi / 2
alpha3_4 = -pi / 2
alpha4_5 = pi / 2
alpha5_6 = -pi / 2
DH_Table = {
alpha0: 0,
a0: 0,
d1: d0_1,
q1: q1,
alpha1: alpha1_2,
a1: a1_2,
d2: 0,
q2: -pi / 2. + q2,
alpha2: 0,
a2: a2_3,
d3: 0,
q3: q3,
alpha3: alpha3_4,
a3: a3_4,
d4: d3_4,
q4: q4,
alpha4: alpha4_5,
a4: 0,
d5: 0,
q5: q5,
alpha5: alpha5_6,
a5: 0,
d6: 0,
q6: q6,
alpha6: 0,
a6: 0,
d7: d6_7,
q7: 0
}
# Define Modified DH Transformation matrix
def TF_Matrix(alpha, a, d, q):
TF = Matrix([[cos(q), -sin(q), 0, a],
[
sin(q) * cos(alpha),
cos(q) * cos(alpha), -sin(alpha), -sin(alpha) * d
],
[
sin(q) * sin(alpha),
cos(q) * sin(alpha),
cos(alpha),
cos(alpha) * d
], [0, 0, 0, 1]])
return TF
# Create individual transformation matrices
T0_1 = TF_Matrix(alpha0, a0, d1, q1).subs(DH_Table)
T1_2 = TF_Matrix(alpha1, a1, d2, q2).subs(DH_Table)
T2_3 = TF_Matrix(alpha2, a2, d3, q3).subs(DH_Table)
T3_4 = TF_Matrix(alpha3, a3, d4, q4).subs(DH_Table)
T4_5 = TF_Matrix(alpha4, a4, d5, q5).subs(DH_Table)
T5_6 = TF_Matrix(alpha5, a5, d6, q6).subs(DH_Table)
T6_7 = TF_Matrix(alpha6, a6, d7, q7).subs(DH_Table)
T0_7 = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_7
# Extract rotation matrices from the transformation matrices
R0_1 = T0_1[0:3, 0:3]
R1_2 = T1_2[0:3, 0:3]
R2_3 = T2_3[0:3, 0:3]
R3_4 = T3_4[0:3, 0:3]
R4_5 = T4_5[0:3, 0:3]
R5_6 = T5_6[0:3, 0:3]
R6_7 = T6_7[0:3, 0:3]
R0_7 = T0_7[0:3, 0:3]
# Initialize service response
joint_trajectory_list = []
for x in xrange(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
### Your IK code here
# Compensate for rotation discrepancy between DH parameters and Gazebo
r, p, y = symbols('r p y')
ROT_x = Matrix([[1, 0, 0], [0, cos(r), -sin(r)],
[0, sin(r), cos(r)]]) # Roll
ROT_y = Matrix([[cos(p), 0, sin(p)], [0, 1, 0],
[-sin(p), 0, cos(p)]]) # Pitch
ROT_z = Matrix([[cos(y), -sin(y), 0], [sin(y), cos(y), 0],
[0, 0, 1]]) # Pitch
ROT_EE = ROT_z * ROT_y * ROT_x
ROT_err = ROT_z.subs(y, radians(180)) * ROT_y.subs(p, radians(-90))
ROT_EE = ROT_EE * ROT_err
ROT_EE = ROT_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
# Calculate joint angles using Geometric IK method
EE = Matrix([[px], [py], [pz]])
WC = EE - (0.303) * ROT_EE[:, 2]
theta1 = atan2(WC[1], WC[0])
side_a = 1.501
side_b = sqrt(
pow((sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35), 2) +
pow((WC[2] - 0.75), 2))
side_c = 1.25
angle_a = acos(
(side_b * side_b + side_c * side_c - side_a * side_a) /
(2 * side_b * side_c))
angle_b = acos(
(side_a * side_a + side_c * side_c - side_b * side_b) /
(2 * side_a * side_c))
angle_c = acos(
(side_a * side_a + side_b * side_b - side_c * side_c) /
(2 * side_a * side_b))
theta2 = pi / 2 - angle_a - atan2(
WC[2] - 0.75,
sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35)
theta3 = pi / 2 - angle_b - 0.036
R0_3 = R0_1 * R1_2 * R2_3
R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
R3_6 = R0_3.inv("LU") * ROT_EE
theta4 = atan2(R3_6[2, 2], -R3_6[0, 2])
theta5 = atan2(
sqrt(R3_6[0, 2] * R3_6[0, 2] + R3_6[2, 2] * R3_6[2, 2]),
R3_6[1, 2])
theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])
###
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
joint_trajectory_point.positions = [
theta1, theta2, theta3, theta4, theta5, theta6
]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
def handle_calculate_IK(req):
rospy.loginfo("Received poses" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses"
return -1
else:
joint_trajectory_list = []
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols('alpha0:7')
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
s = {alpha0: 0, a0: 0, d1: 0.75,
alpha1: -pi/2, a1: 0.35, d2: 0, q2: q2 - pi/2,
alpha2: 0, a2: 1.25, d3: 0,
alpha3: -pi/2, a3: -0.054, d4: 1.5,
alpha4: pi/2, a4: 0, d5: 0,
alpha5: -pi/2, a5: 0, d6: 0,
alpha6: 0, a6: 0, d7: 0.303, q7: 0}
T0_1 = Matrix([[ cos(q1), -sin(q1), 0, a0],
[ sin(q1)*cos(alpha0), cos(q1)*cos(alpha0), -sin(alpha0), -sin(alpha0)*d1],
[ sin(q1)*sin(alpha0), cos(q1)*sin(alpha0), cos(alpha0), cos(alpha0)*d1],
[ 0, 0, 0, 1]])
T0_1 = T0_1.subs(s)
T1_2 = Matrix([[ cos(q2), -sin(q2), 0, a1],
[ sin(q2)*cos(alpha1), cos(q2)*cos(alpha1), -sin(alpha1), -sin(alpha1)*d2],
[ sin(q2)*sin(alpha1), cos(q2)*sin(alpha1), cos(alpha1), cos(alpha1)*d2],
[ 0, 0, 0, 1]])
T1_2 = T1_2.subs(s)
T2_3 = Matrix([[ cos(q3), -sin(q3), 0, a2],
[ sin(q3)*cos(alpha2), cos(q3)*cos(alpha2), -sin(alpha2), -sin(alpha2)*d3],
[ sin(q3)*sin(alpha2), cos(q3)*sin(alpha2), cos(alpha2), cos(alpha2)*d3],
[ 0, 0, 0, 1]])
T2_3 = T2_3.subs(s)
T3_4 = Matrix([[ cos(q4), -sin(q4), 0, a3],
[ sin(q4)*cos(alpha3), cos(q4)*cos(alpha3), -sin(alpha3), -sin(alpha3)*d4],
[ sin(q4)*sin(alpha3), cos(q4)*sin(alpha3), cos(alpha3), cos(alpha3)*d4],
[ 0, 0, 0, 1]])
T3_4 = T3_4.subs(s)
T4_5 = Matrix([[ cos(q5), -sin(q5), 0, a4],
[ sin(q5)*cos(alpha4), cos(q5)*cos(alpha4), -sin(alpha4), -sin(alpha4)*d5],
[ sin(q5)*sin(alpha4), cos(q5)*sin(alpha4), cos(alpha4), cos(alpha4)*d5],
[ 0, 0, 0, 1]])
T4_5 = T4_5.subs(s)
T5_6 = Matrix([[ cos(q6), -sin(q6), 0, a5],
[ sin(q6)*cos(alpha5), cos(q6)*cos(alpha5), -sin(alpha5), -sin(alpha5)*d6],
[ sin(q6)*sin(alpha5), cos(q6)*sin(alpha5), cos(alpha5), cos(alpha5)*d6],
[ 0, 0, 0, 1]])
T5_6 = T5_6.subs(s)
T6_7 = Matrix([[ cos(q7), -sin(q7), 0, a6],
[ sin(q7)*cos(alpha6), cos(q7)*cos(alpha6), -sin(alpha6), -sin(alpha6)*d7],
[ sin(q7)*sin(alpha6), cos(q7)*sin(alpha6), cos(alpha6), cos(alpha6)*d7],
[ 0, 0, 0, 1]])
T6_7 = T6_7.subs(s)
T0_7 = simplify(T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_7)
R_z = Matrix([[ cos(pi), -sin(pi), 0, 0],
[ sin(pi), cos(pi), 0, 0],
[ 0, 0, 1, 0],
[ 0, 0, 0, 1]])
R_y = Matrix([[ cos(-pi/2), 0, sin(-pi/2), 0],
[ 0, 1, 0, 0],
[ -sin(-pi/2), 0, cos(-pi/2), 0],
[ 0, 0, 0, 1]])
R_corr = simplify(R_z * R_y)
T0_3 = simplify(T0_1 * T1_2 * T2_3)
R0_3 = T0_3[0:3, 0:3]
r, p, ya = symbols('r p ya')
R_roll = Matrix([[ 1, 0, 0],
[ 0, cos(r), -sin(r)],
[ 0, sin(r), cos(r)]])
R_pitch = Matrix([[ cos(p), 0, sin(p)],
[ 0, 1, 0],
[-sin(p), 0, cos(p)]])
R_yaw = Matrix([[ cos(ya), -sin(ya), 0],
[ sin(ya), cos(ya), 0],
[ 0, 0, 1]])
R0_6 = simplify(R_yaw * R_pitch * R_roll)
R0_6 = simplify(R0_6 * R_corr[0:3, 0:3])
for x in xrange(0, len(req.poses)):
joint_trajectory_point = JointTrajectoryPoint()
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(
[req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w])
J5__0 = [1.85, 0, 1.946]
R0_6_num = R0_6.evalf(subs={r:roll, ya:yaw, p:pitch})
P_EE = Matrix([[px],[py],[pz]])
P_WC = P_EE - 0.303 * R0_6_num * Matrix([[0],[0],[1]])
J5 = P_WC
theta1 = atan2(J5[1], J5[0])
J2__0 = [0.35, 0, 0.75]
J3__0 = [0.35, 0, 2]
J5__0 = [1.85, 0, 1.946]
J2 = [J2__0[0] * cos(theta1), J2__0[0] * sin(theta1), J2__0[2]]
L2_5_X = J5[0] - J2[0]
L2_5_Y = J5[1] - J2[1]
L2_5_Z = J5[2] - J2[2]
L2_5 = sqrt(L2_5_X**2 + L2_5_Y**2 + L2_5_Z**2)
L2_3__0 = 1.25
L3_5_X__0 = J5__0[0] - J3__0[0]
L3_5_Z__0 = J5__0[2] - J3__0[2]
L3_5__0 = sqrt(L3_5_X__0**2 + L3_5_Z__0**2)
D = (L2_5**2 - L2_3__0**2 - L3_5__0**2) / -(2 * L2_3__0 * L3_5__0)
theta3_internal = atan2(sqrt(1-D**2), D)
theta3 = pi / 2 - theta3_internal + atan2(L3_5_Z__0, L3_5_X__0)
m1 = L3_5__0 * sin(theta3_internal)
m2 = L2_3__0 - L3_5__0 * cos(theta3_internal)
b2 = atan2(m1,m2)
b1 = atan2(J5[2]-J2[2], sqrt((J5[0]-J2[0])**2 + (J5[1]-J2[1])**2))
theta2 = pi / 2 - b2 - b1
R0_3_num = R0_3.evalf(subs={q1:theta1, q2:theta2, q3:theta3})
R0_3_num_inv = R0_3_num.inv("LU")
R3_6 = R0_3_num_inv * R0_6_num
theta4 = atan2(R3_6[2,2], -R3_6[0,2])
theta5 = atan2(sqrt(R3_6[0,2]*R3_6[0,2] + R3_6[2,2]*R3_6[2,2]), R3_6[1,2])
theta6 = atan2(-R3_6[1,1], R3_6[1,0])
joint_trajectory_point.positions = [theta1, theta2, theta3, theta4, theta5, theta6]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
### Your FK code here
# Create symbols
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') #joint variable theta_i
d1, d2, d3, d4, d5, d6, d7 = symbols(
'd1:8') #link offset dist(x_{i-1}, x_{i})
a0, a1, a2, a3, a4, a5, a6 = symbols(
'a0:7') #link length dist(z_{i-1}, z_{i})
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols(
'alpha0:7') #twist angle ang <z_{i-1}, z_{i}>
# Create Modified DH parameters
# Create Modified DH parameters
DH_table = {
alpha0: 0,
a0: 0,
d1: 0.75,
q1: q1,
alpha1: -pi / 2.,
a1: 0.35,
d2: 0,
q2: q2 - pi / 2.,
alpha2: 0,
a2: 1.25,
d3: 0,
q3: q3,
alpha3: -pi / 2.,
a3: -0.054,
d4: 1.50,
q4: q4,
alpha4: pi / 2.,
a4: 0,
d5: 0,
q5: q5,
alpha5: -pi / 2.,
a5: 0,
d6: 0,
q6: q6,
alpha6: 0,
a6: 0,
d7: 0.303,
q7: 0
}
joint_angle_limit_theta1 = [-185, 185]
joint_angle_limit_theta2 = [-45, 85]
joint_angle_limit_theta3 = [-210, 65]
joint_angle_limit_theta4 = [-350, 350]
joint_angle_limit_theta5 = [-125, 125]
joint_angle_limit_theta6 = [-350, 250]
# Define Modified DH Transformation matrix
T0_1 = HomTransf_ij(alpha0, a0, d1,
q1) #Transform of frame1 wrt frame0
T0_1 = T0_1.subs(DH_table)
T1_2 = HomTransf_ij(alpha1, a1, d2, q2)
T1_2 = T1_2.subs(DH_table)
T2_3 = HomTransf_ij(alpha2, a2, d3, q3)
T2_3 = T2_3.subs(DH_table)
T3_4 = HomTransf_ij(alpha3, a3, d4, q4)
T3_4 = T3_4.subs(DH_table)
T4_5 = HomTransf_ij(alpha4, a4, d5, q5)
T4_5 = T4_5.subs(DH_table)
T5_6 = HomTransf_ij(alpha5, a5, d6, q6)
T5_6 = T5_6.subs(DH_table)
T6_G = HomTransf_ij(alpha6, a6, d7, q7)
T6_G = T6_G.subs(DH_table)
#Post-multiplication: Forward kinematic
T0_2 = T0_1 * T1_2 #transform base link frame to link2 frame
T0_3 = T0_2 * T2_3 #transform base link to link3
T0_4 = T0_3 * T3_4 #transform base link to link4
T0_5 = T0_4 * T4_5 #transform base link to link5
T0_6 = T0_5 * T5_6 #transform base link to link6
T0_G = T0_6 * T6_G #transform base link to Gripper
# Initialize service response
joint_trajectory_list = []
# create Gripper rotation matrix:
r, p, y = symbols('r p y')
R_G = Rot(y, axis='z') * Rot(p, axis='y') * Rot(
r, axis='x') #in urdf frame
#rotation correction to align urdf frame with DH frame -- works also to align DH frame with urdf
p_corr, y_corr = symbols('p_corr y_corr')
R_corr = Rot(y_corr, axis='z') * Rot(p_corr, axis='y')
R_corr = R_corr.subs({'p_corr': -pi / 2, 'y_corr': pi})
results = "step q1 q2 q3 q4 q5 q6\n"
for step_idx, x in enumerate(xrange(0, len(req.poses))):
print("Trajectory step #", str(step_idx))
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
position_G = Matrix([[px], [py], [pz]])
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
### Your IK code here
#Evaluation of Gripper orientation in urdf frame
R_G = R_G.subs({'r': roll, 'p': pitch, 'y': yaw})
#Apply correction
R_G = R_G * R_corr
#Calculate wrist center position
dG = 0.303 #distance (WC, O_G)=d7 in DH table
WC = position_G - dG * R_G[:,
2] #last term is projection of z axis of Gripper frame on axis of reference frame zero
theta1 = atan2(WC[1], WC[0]).evalf()
#theta1 = clip_angle(theta1, joint_angle_limit_theta1)
side_a = 1.501
side_b = sqrt((sqrt(WC[0]**2 + WC[1]**2) - 0.35)**2 +
(WC[2] - 0.75)**2)
side_c = 1.25
#use cosine laws
cos_a = (side_b**2 + side_c**2 - side_a**2) / (2 * side_b * side_c)
#sin_a = (1 - cos_a * cos_a)**0.5 #there are 2 colutions +/- sin_a. Chose +sin_a
#angle_a = atan2(sin_a, cos_a)
angle_a = acos(
cos_a) #there are 2 solutions pos or neg. (cos(a) = cos(-a))
cos_b = (side_a**2 + side_c**2 - side_b**2) / (2 * side_a * side_c)
#sin_b = (1- cos_b * cos_b)**0.5 # there are 2 solutions +/- . Arbitraily select +sin_b
#angle_b = atan2( sin_b, cos_b )
angle_b = acos(cos_b) #2 possible solution (cos(a) = cos(-a))
theta2 = pi / 2. - angle_a - atan2(
WC[2] - 0.75,
sqrt(WC[0]**2 + WC[1]**2) - 0.35)
theta2 = theta2.evalf()
#theta2 = clip_angle(theta2, joint_angle_limit_theta2)
theta3 = pi / 2. - (angle_b + 0.036)
theta3 = theta3.evalf()
#theta3 = clip_angle(theta3, joint_angle_limit_theta3)
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
#
if step_idx == 0:
prev_theta1, prev_angle_a, prev_angle_b = theta1, angle_a, angle_b
else:
#validate angles
angle_a = validate(angle_a, prev_angle_a)
angle_b = validate(angle_b, prev_angle_b)
prev_theta1, prev_angle_a, prev_angle_b = theta1, angle_a, angle_b
#recompute theta2 and theta3
theta2 = pi / 2. - angle_a - atan2(
WC[2] - 0.75,
sqrt(WC[0]**2 + WC[1]**2) - 0.35)
theta2 = theta2.evalf()
#theta2 = clip_angle(theta2, joint_angle_limit_theta2)
theta3 = pi / 2. - (angle_b + 0.036)
theta3 = theta3.evalf()
#Get rotation matrix of frame 3 wrt frame 0
R0_3 = T0_3[
0:3, 0:
3] #extract from Homogeneous transf matrix, the matrix components related to orientation (3x3 matrix)
R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
R3_6 = R0_3.inv("LU") * R_G
#Euler angles from rotation matrix
theta4 = atan2(R3_6[2, 2], -R3_6[0, 2]).evalf()
#theta4 = clip_angle(theta4, joint_angle_limit_theta4)
theta5 = atan2(sqrt(R3_6[0, 2]**2 + R3_6[2, 2]**2),
R3_6[1, 2]).evalf() #at least 2 solutions
#if step_idx == 0:
# prev_theta5 = theta5
#else:
#validate theta5
# test_theta5 = atan2( -sqrt(R3_6[0,2]**2 + R3_6[2,2]**2), R3_6[1,2]).evalf()
# if abs(theta5 - prev_theta5) > abs(test_theta5 - prev_theta5):
# theta5 = test_theta5
#theta5 = clip_angle(theta4, joint_angle_limit_theta5)
theta6 = atan2(-R3_6[1, 1], R3_6[1, 0]).evalf()
#theta6 = clip_angle(theta4, joint_angle_limit_theta6)
joint_trajectory_point.positions = [
theta1, theta2, theta3, theta4, theta5, theta6
]
joint_trajectory_list.append(joint_trajectory_point)
print(" ***Angles: {} | {} | {} | {} | {} | {}".format( degrees(theta1), degrees(theta2), \
degrees(theta3), degrees(theta4), \
degrees(theta5), degrees(theta6) ) )
results += str(step_idx) + " " + str(degrees(theta1)) + " " + str(
degrees(theta2)) + " " + str(degrees(theta3)) + " " + str(
degrees(theta4)) + " " + str(degrees(theta5)) + " " + str(
degrees(theta6)) + "\n"
#Compute the forward Kinematic
T_FK = T0_G.evalf(
subs={
q1: theta1,
q2: theta2,
q3: theta3,
q4: theta4,
q5: theta5,
q6: theta6
})
with open("IK_angles_per_step.txt", "w") as text_file:
text_file.write(results)
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
# -------------------------------------------------------------------------------------------------------------
# FK code here
# Create symbols
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols('alpha0:7')
# Create Modified DH parameters
s = {alpha0: 0, a0: 0, d1: 0.75, q1: q1,
alpha1: -pi/2, a1: 0.35, d2: 0, q2: q2 - pi/2,
alpha2: 0, a2: 1.25, d3: 0, q3: q3,
alpha3: -pi/2, a3: -0.054, d4: 1.50, q4: q4,
alpha4: pi/2, a4: 0, d5: 0, q5: q5,
alpha5: -pi/2, a5: 0, d6: 0, q6: q6,
alpha6: 0, a6: 0, d7: 0.303, q7: 0}
# Individual Transformations
# Homogeneuos Transformation Link_0 to link_1
T0_1 = Matrix([[cos(q1), -sin(q1), 0, a0],
[sin(q1)*cos(alpha0), cos(q1)*cos(alpha0), -sin(alpha0), -sin(alpha0)*d1],
[sin(q1)*sin(alpha0), cos(q1)*sin(alpha0), cos(alpha0), cos(alpha0)*d1],
[0, 0, 0, 1]])
T0_1 = T0_1.subs(s)
# Homogeneuos Transformation Link_1 to link_2
T1_2 = Matrix([[cos(q2), -sin(q2), 0, a1],
[sin(q2)*cos(alpha1), cos(q2)*cos(alpha1), -sin(alpha1), -sin(alpha1)*d2],
[sin(q2)*sin(alpha1), cos(q2)*sin(alpha1), cos(alpha1), cos(alpha1)*d2],
[0, 0, 0, 1]])
T1_2 = T1_2.subs(s)
# Homogeneuos Transformation Link_2 to link_3
T2_3 = Matrix([[cos(q3), -sin(q3), 0, a2],
[sin(q3)*cos(alpha2), cos(q3)*cos(alpha2), -sin(alpha2), -sin(alpha2)*d3],
[sin(q3)*sin(alpha2), cos(q3)*sin(alpha2), cos(alpha2), cos(alpha2)*d3],
[0, 0, 0, 1]])
T2_3 = T2_3.subs(s)
# Homogeneuos Transformation Link_3 to link_4
T3_4 = Matrix([[cos(q4), -sin(q4), 0, a3],
[sin(q4)*cos(alpha3), cos(q4)*cos(alpha3), -sin(alpha3), -sin(alpha3)*d4],
[sin(q4)*sin(alpha3), cos(q4)*sin(alpha3), cos(alpha3), cos(alpha3)*d4],
[0, 0, 0, 1]])
T3_4 = T3_4.subs(s)
# Homogeneuos Transformation Link_4 to link_5
T4_5 = Matrix([[cos(q5), -sin(q5), 0, a4],
[sin(q5)*cos(alpha4), cos(q5)*cos(alpha4), -sin(alpha4), -sin(alpha4)*d5],
[sin(q5)*sin(alpha4), cos(q5)*sin(alpha4), cos(alpha4), cos(alpha4)*d5],
[0, 0, 0, 1]])
T4_5 = T4_5.subs(s)
# Homogeneuos Transformation Link_5 to link_6
T5_6 = Matrix([[cos(q6), -sin(q6), 0, a5],
[sin(q6)*cos(alpha5), cos(q6)*cos(alpha5), -sin(alpha5), -sin(alpha5)*d6],
[sin(q6)*sin(alpha5), cos(q6)*sin(alpha5), cos(alpha5), cos(alpha5)*d6],
[0, 0, 0, 1]])
T5_6 = T5_6.subs(s)
# Homogeneuos Transformation Link_6 to link_7 (Gripper)
T6_G = Matrix([[cos(q7), -sin(q7), 0, a6],
[sin(q7)*cos(alpha6), cos(q7)*cos(alpha6), -sin(alpha6), -sin(alpha6)*d7],
[sin(q7)*sin(alpha6), cos(q7)*sin(alpha6), cos(alpha6), cos(alpha6)*d7],
[0, 0, 0, 1]])
T6_G = T6_G.subs(s)
# Transform from Base link to end effector (Gripper)
# Important: If we multiply in conjunction the result is different.
T0_2 = (T0_1 * T1_2) # Link_0 to Link_2
T0_3 = (T0_2 * T2_3) # Link_0 to Link_3
T0_4 = (T0_3 * T3_4) # Link_0 to Link_4
T0_5 = (T0_4 * T4_5) # Link_0 to Link_5
T0_6 = (T0_5 * T5_6) # Link_0 to Link_6
T0_7 = (T0_6 * T6_G) # Link_0 to Link_7
# Initialize service response
joint_trajectory_list = []
for x in xrange(0, len(req.poses)):
joint_trajectory_point = JointTrajectoryPoint()
# Your IK code here
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
# Saving the EE position in a matrix
EE = Matrix([[px],
[py],
[pz]])
# Requested end-effector orientation
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(
[req.poses[x].orientation.x,
req.poses[x].orientation.y,
req.poses[x].orientation.z,
req.poses[x].orientation.w])
# Creating symbols for the rotation matrices (Roll, Pitch, Yaw)
r, p, y = symbols('r p y')
# Roll
ROT_x = Matrix([[1, 0, 0],
[0, cos(r), -sin(r)],
[0, sin(r), cos(r)]])
# Pitch
ROT_y = Matrix([[cos(p), 0, sin(p)],
[0, 1, 0],
[-sin(p), 0, cos(p)]])
# Yaw
ROT_z = Matrix([[cos(y), -sin(y), 0],
[sin(y), cos(y), 0],
[0, 0, 1]])
# The rotation matrix amoung the 3 axis
ROT_EE = ROT_z * ROT_y * ROT_x
# Correction Needed to Account for Orientation Difference Between
# Definition of Gripper Link_G in URDF versus DH Convention
ROT_corr = ROT_z.subs(y, radians(180)) * ROT_y.subs(p, radians(-90))
ROT_EE = ROT_EE * ROT_corr
ROT_EE = ROT_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
# Calculate Wrest Center
WC = EE - (0.303) * ROT_EE[:, 2]
# Calculate joint angles
# Calculate theat1
theta1 = atan2(WC[1], WC[0])
# find the 3rd side of the triangle
A = 1.50
C = 1.25
B = sqrt(pow((sqrt(WC[0]*WC[0] + WC[1]*WC[1]) - 0.35), 2) + pow((WC[2] - 0.75), 2))
# Cosine Laws SSS to find all inner angles of the triangle
a = acos((B*B + C*C - A*A) / (2*B*C))
b = acos((A*A + C*C - B*B) / (2*A*C))
c = acos((A*A + B*B - C*C) / (2*A*B))
# Find theta2 and theta3
theta2 = pi/2 - a - atan2(WC[2]-0.75, sqrt(WC[0]*WC[0]+WC[1]*WC[1])-0.35)
theta3 = pi/2 - (b+0.036)
# Extract rotation matrix R0_3 from transformation matrix T0_3 the substitute angles q1-3
R0_3 = T0_1[0:3, 0:3] * T1_2[0:3, 0:3] * T2_3[0:3, 0:3]
R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
# Get rotation matrix R3_6 from (transpose of R0_3 * R_EE)
R3_6 = R0_3.transpose() * ROT_EE
# Euler angles from rotation matrix
theta4 = atan2(R3_6[2, 2], -R3_6[0, 2])
theta5 = atan2(sqrt(R3_6[0, 2]*R3_6[0, 2] + R3_6[2, 2]*R3_6[2, 2]), R3_6[1, 2])
theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
joint_trajectory_point.positions = [theta1, theta2, theta3, theta4, theta5, theta6]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
# Create symbols
# Link Offset
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
# Link Length
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
# Twist Angle
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols('alpha0:7')
# Joint Angle
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
# Create Modified DH parameters
DH_Table = {alpha0: 0, a0: 0, d1: 0.75, q1: q1,
alpha1: -pi/2., a1: 0.35, d2: 0, q2: -pi/2 + q2,
alpha2: 0, a2: 1.25, d3: 0, q3: q3,
alpha3: -pi/2., a3: -0.054, d4: 1.5, q4: q4,
alpha4: -pi/2., a4: 0, d5: 0, q5: q5,
alpha5: -pi/2., a5: 0, d6: 0, q6: q6,
alpha6: 0, a6: 0, d7: 0.303, q7: 0}
# Define Modified DH Transformation matrix
def TF_Matrix(alpha, a, d, q):
TF = Matrix([
[ cos(q), -sin(q), 0, a],
[sin(q)*cos(alpha), cos(q)*cos(alpha), -sin(alpha), -sin(alpha)*d],
[sin(q)*sin(alpha), cos(q)*sin(alpha), cos(alpha), cos(alpha)*d],
[ 0, 0, 0, 1]])
return TF
# Create individual transformation matrices
T0_1 = TF_Matrix(alpha0, a0, d1, q1).subs(DH_Table)
T1_2 = TF_Matrix(alpha1, a1, d2, q2).subs(DH_Table)
T2_3 = TF_Matrix(alpha2, a2, d3, q3).subs(DH_Table)
T3_4 = TF_Matrix(alpha3, a3, d4, q4).subs(DH_Table)
T4_5 = TF_Matrix(alpha4, a4, d5, q5).subs(DH_Table)
T5_6 = TF_Matrix(alpha5, a5, d6, q6).subs(DH_Table)
T6_EE = TF_Matrix(alpha6, a6, d7, q7).subs(DH_Table)
# Tranform matrix from base to end effector
T0_EE = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_EE
# Define Axis rotation matrix
r, p, y = symbols('r p y')
# Roll
ROT_x = Matrix([[1, 0, 0],
[0, cos(r), -sin(r)],
[0, sin(r), cos(r)]])
# Pitch
ROT_y = Matrix([[ cos(p), 0, sin(p)],
[ 0, 1, 0],
[-sin(p), 0, cos(p)]])
# Yaw
ROT_z = Matrix([[cos(y), -sin(y), 0],
[sin(y), cos(y), 0],
[0, 0, 1]])
# Correctional rotation matrix
Rot_corr = ROT_z.subs(y, pi) * ROT_y.subs(p, -pi/2)
# EE Rotation
ROT_EE = ROT_z * ROT_y * ROT_x
# Compensate for rotation discrepancy between DH parameters and Gazebo
ROT_EE = ROT_EE * Rot_corr
# Compute inverse of rotation matrix R0_3 preparing for solving
# inverse orientation problem
R0_3 = T0_1[0:3,0:3] * T1_2[0:3,0:3] * T2_3[0:3,0:3]
R0_3_INV = R0_3.T
#R0_3_INV = R0_3.inv("LU")
# Initialize service response
joint_trajectory_list = []
for x in xrange(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
### Your FK code here
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(
[req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w])
# STEP 1: Solve inverse position problem
# Compute WC position
ROT_ee = ROT_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
EE = Matrix([[px],
[py],
[pz]])
WC = EE - (0.303) * ROT_ee[:,2]
# Calculate joint angles using Geometric IK method
wc_proj = sqrt(WC[0]**2 + WC[1]**2)
side_a = 1.501
side_b = sqrt(pow((wc_proj - 0.35), 2) + pow((WC[2] - 0.75), 2))
side_c = 1.25
angle_a = acos((side_b**2 + side_c**2 - side_a**2) / (2 * side_b * side_c))
angle_b = acos((side_a**2 + side_c**2 - side_b**2) / (2 * side_a * side_c))
angle_c = acos((side_a**2 + side_b**2 - side_c**2) / (2 * side_a * side_b))
theta1 = atan2(WC[1], WC[0])
theta2 = pi / 2 - angle_a - atan2(WC[2] - 0.75, wc_proj - 0.35)
theta3 = pi / 2 - (angle_b + 0.036)
# STEP 2: Solve inverse orientation problem
R0_3_inv = R0_3_INV.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
R3_6 = R0_3_inv * ROT_ee
theta5 = atan2(sqrt(R3_6[0, 2]**2 + R3_6[2, 2]**2), R3_6[1, 2])
theta4 = atan2(R3_6[2, 2], -R3_6[0, 2])
theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])
joint_trajectory_point.positions = [theta1, theta2, theta3, theta4, theta5, theta6]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
# Initialize service response
joint_trajectory_list = []
last_theta4 = 0
last_theta5 = 0
for x in xrange(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
quart = [
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
]
# Calculate joint angles using Geometric IK method
wc_coord = to_wc([px, py, pz], quart) #get wrist center
R0_6 = get_R0_6(*quart) #get Rrpy
found = False
theta_one_possibilities = get_theta_one(wc_coord)
for theta1 in theta_one_possibilities:
T1_shift_wc_coord = simplify(T1_0_shift *
wc_coord).evalf(subs={q1: theta1})
theta_2_3_pairs = get_theta2_pairs(T1_shift_wc_coord)
for pair in theta_2_3_pairs:
theta2, theta3 = pair
R0_3 = T0_3[0:3, 0:3].subs({
q1: theta1,
q2: theta2,
q3: theta3
}) * rot_x(pi / 2) * rot_y(pi / 2)
R3_0 = Inverse(R0_3)
final_mat = R3_0 * R0_6
theta5 = acos(final_mat[0, 0])
mult = -1 if theta5 < 0 else 1
theta4 = atan2(mult * -final_mat[1, 0], final_mat[2, 0])
test_ans = sin(theta5) * sin(theta4)
if not abs(test_ans - final_mat[1, 0] / test_ans) < .002:
"""
if abs(last_theta4 - (theta4 + pi)) < abs(last_theta4 - (theta4 - pi)):
theta4 += pi
else:
theta4 -= pi
"""
if theta4 > 0:
theta4 -= pi
else:
theta4 += pi
last_theta4 = theta4
theta6 = atan2(final_mat[0, 1], final_mat[0, 2])
last_theta5 = theta5
if all([t.is_real for t in [theta4, theta5, theta6]]):
found = True
break
if found:
break
if not all([t.is_real for t in [theta4, theta5, theta6]]):
print[theta1, theta2, theta3, theta4, theta5, theta6]
print[px, py, pz]
print quart
raise Exception
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
print[theta1, theta2, theta3, theta4, theta5, theta6]
joint_trajectory_point.positions = [
theta1.evalf(),
theta2.evalf(),
theta3.evalf(),
theta4.evalf(),
theta5.evalf(),
theta6.evalf()
]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
def send_trajectory_one_by_one(self, time_duration):
"""DIFFERENT from `send_trajectory`, send a goal which only contains ONE trajectory point to the action server, iterating for len(self._time_list)-1 times
"""
rospy.loginfo("Start going to other trajectory points one by one")
time_last = rospy.Time.now()
# since the first pt has been realized, iteration will start from the second pt
for traj_idx in range(len(self._time_list) - 1):
# a goal to be sent to action server
# this goal will contain only one trajectory point
goal = FollowJointTrajectoryGoal()
# add joint name
goal.trajectory.joint_names.append('base_x')
goal.trajectory.joint_names.append('x_y')
goal.trajectory.joint_names.append('y_car')
for idx in range(self._joint_num):
goal.trajectory.joint_names.append("joint_a" + str(idx + 1))
# a joint point in the trajectory
trajPt = JointTrajectoryPoint()
trajPt.positions.append(self._joints_pos_dict['base_x'][idx + 1])
trajPt.positions.append(self._joints_pos_dict['x_y'][idx + 1])
trajPt.positions.append(self._joints_pos_dict['y_car'][idx + 1])
for idx in range(self._joint_num):
joint_name = "joint_a" + str(idx + 1)
trajPt.positions.append(
self._joints_pos_dict[joint_name][traj_idx + 1])
#trajPt.velocities.append(0.0)
# time to reach the joint trajectory point specified to 1.0 since this will be controlled by my enter
trajPt.time_from_start = rospy.Duration(secs=time_duration)
# add the joint trajectory point to the goal
goal.trajectory.points.append(trajPt)
# rospy.loginfo("At iteration {} goal has {} points to reach".format(traj_idx ,len(goal.trajectory.points)))
# rospy.loginfo("each of them is ")
for idx in range(len(goal.trajectory.points)):
print goal.trajectory.points[idx]
goal.trajectory.header.stamp = rospy.Time.now()
# send the goal to the action server
self._action_client.send_goal(goal)
# wait for the result
rospy.loginfo("Start waiting for go to the next pose")
self._action_client.wait_for_result()
rospy.loginfo("Waiting ends")
# show the error code
rospy.loginfo(self._action_client.get_result())
# show the time used to move to current position
time_next = rospy.Time.now()
rospy.loginfo("At iteration {}, time Duration is {}".format(
traj_idx, (time_next - time_last).to_sec()))
time_last = time_next
# uncomment raw_input() if you want to control the pace of sending goals to the action server
raw_input()
def handle_calculate_IK(req):
"""Handle request from a CalculateIK type service."""
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
dh = get_DH_Table()
# Initialize service response consisting of a list of
# joint trajectory positions (joint angles) corresponding
# to a given gripper pose
joint_trajectory_list = []
# To store coordinates for plotting (in plot_ee() function)
received_ee_points = []
fk_ee_points = []
ee_errors = []
# For each gripper pose a response of six joint angles is computed
len_poses = len(req.poses)
for x in xrange(0, len_poses):
joint_trajectory_point = JointTrajectoryPoint()
# INVERSE KINEMATICS
ee_pose = get_ee_pose(req.poses[x])
received_ee_points.append(ee_pose[0])
R_ee = get_R_EE(ee_pose)
Wc = get_WC(dh, R_ee, ee_pose)
# Calculate angles for joints 1,2,3 and update dh table
theta1, theta2, theta3 = get_joints1_2_3(dh, Wc)
dh['theta1'] = theta1
dh['theta2'] = theta2 - pi / 2 # account for 90 deg constant offset
dh['theta3'] = theta3
# Calculate angles for joints 4,5,6 and update dh table
theta4, theta5, theta6 = get_joints4_5_6(dh, R_ee, theta1, theta2,
theta3)
dh['theta4'] = theta4
dh['theta5'] = theta5
dh['theta6'] = theta6
# Populate response for the IK request
joint_trajectory_point.positions = [
theta1, theta2, theta3, theta4, theta5, theta6
]
joint_trajectory_list.append(joint_trajectory_point)
def calculate_FK():
"""Calculate Forward Kinematics for verifying joint angles."""
# Compute individual transforms between adjacent links
# T(i-1)_i = Rx(alpha(i-1)) * Dx(alpha(i-1)) * Rz(theta(i)) * Dz(d(i))
T0_1 = get_TF(dh['alpha0'], dh['a0'], dh['d1'], dh['theta1'])
T1_2 = get_TF(dh['alpha1'], dh['a1'], dh['d2'], dh['theta2'])
T2_3 = get_TF(dh['alpha2'], dh['a2'], dh['d3'], dh['theta3'])
T3_4 = get_TF(dh['alpha3'], dh['a3'], dh['d4'], dh['theta4'])
T4_5 = get_TF(dh['alpha4'], dh['a4'], dh['d5'], dh['theta5'])
T5_6 = get_TF(dh['alpha5'], dh['a5'], dh['d6'], dh['theta6'])
T6_ee = get_TF(dh['alpha6'], dh['a6'], dh['dG'], dh['thetaG'])
# Create overall transform between base frame and EE by
# composing the individual link transforms
T0_ee = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_ee
fk_ee = [T0_ee[0, 3], T0_ee[1, 3], T0_ee[2, 3]]
fk_ee_points.append([
round(fk_ee[0].item(0), 8),
round(fk_ee[1].item(0), 8),
round(fk_ee[2].item(0), 8)
])
ee_x_e = abs(fk_ee[0] - ee_pose[0][0])
ee_y_e = abs(fk_ee[1] - ee_pose[0][1])
ee_z_e = abs(fk_ee[2] - ee_pose[0][2])
ee_errors.append([
round(ee_x_e.item(0), 8),
round(ee_y_e.item(0), 8),
round(ee_z_e.item(0), 8)
])
# NOTE: Uncomment following line to compute FK for plotting EE
#calculate_FK()
rospy.loginfo("Number of joint trajectory points:" +
" %s" % len(joint_trajectory_list))
# NOTE: Uncomment the following line to plot EE trajectories
#plot_EE(received_ee_points, fk_ee_points, ee_errors)
return CalculateIKResponse(joint_trajectory_list)
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
### Your FK code here
# Create symbols
#
#
# Create Modified DH parameters
#
#
# Define Modified DH Transformation matrix
#
#
# Create individual transformation matrices
#
#
# Extract rotation matrices from the transformation matrices
#
#
###
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') # joint angles
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8') # lik offset
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7') # link lenght
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols(
'alpha0:7') # twist angles
T0_1, T1_2, T2_3, T3_4, T4_5, T5_6, T6_G, T0_EE = kinematics.create_individual_tf_matrices(
q1, q2, q3, q4, q5, q6, q7, d1, d2, d3, d4, d5, d6, d7, a0, a1, a2,
a3, a4, a5, a6, alpha0, alpha1, alpha2, alpha3, alpha4, alpha5,
alpha6)
# Initialize service response
joint_trajectory_list = []
for x in xrange(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
### Your IK code here
# Compensate for rotation discrepancy between DH parameters and Gazebo
#
#
# Calculate joint angles using Geometric IK method
#
#
###
R_ee, WC = kinematics.calculate_wrist_center(
roll, pitch, yaw, px, py, pz)
theta1, theta2, theta3, theta4, theta5, theta6 = kinematics.calculate_thetas(
WC, T0_1, T1_2, T2_3, R_ee, q1, q2, q3, q4, q5, q6, q7)
#if x >= len(req.poses):
#theta5 = theta5_fin
#theta6 = theta6_fin
theta1_fin = theta1
theta2_fin = theta2
theta3_fin = theta3
theta4_fin = theta4
theta5_fin = theta5
theta6_fin = theta6
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
joint_trajectory_point.positions = [
theta1_fin, theta2_fin, theta3_fin, theta4_fin, theta5_fin,
theta6_fin
]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
def _build_segment(self, config):
segment = JointTrajectoryPoint()
segment.positions = config["positions"]
segment.velocities = [0] * len(config["positions"])
segment.time_from_start = rospy.Duration(config["duration"])
return segment
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses" % len(req.poses))
if len(req.poses) < 1:
print "No poses received"
return -1
else:
# Initialize service response
joint_trajectory_list = []
for x in xrange(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
# from the rpy received by this node, we can construct a rotation matrix
# in order to derive the wrist center
R_x = Matrix([[1, 0, 0], [0, cos(roll), -sin(roll)],
[0, sin(roll), cos(roll)]])
R_y = Matrix([[cos(pitch), 0, sin(pitch)], [0, 1, 0],
[-sin(pitch), 0, cos(pitch)]])
R_z = Matrix([[cos(yaw), -sin(yaw), 0], [sin(yaw),
cos(yaw), 0], [0, 0, 1]])
R_rot = R_z * R_y * R_x
# since the quaternion in relative to the urdf definition of the gripper frame, we apply two more rotations to
# the above rotation matrix to get the correct orientation such that the gripper lies along the z axis
R_corr = R_rot * Matrix([[0, 0, 1], [0, -1, 0], [1, 0, 0]])
l = .193 + .11 # distance from the wrist center to the gripper.
# Here the wrist center is assumed to be the location of joint 5, since it is the furthest fixed point
# coincident with joint 3 and in the theta = q1 plane.
d6 = 0
# once we have the correct rotation matrix, we can solve for the position of the wrist center
wc_x = px - (d6 + l) * R_corr[2]
wc_y = py - (d6 + l) * R_corr[5]
wc_z = pz - (d6 + l) * R_corr[8]
# first, the q1 value can be obtained by projecting the wrist center onto the xy plane
theta1 = float(atan2(wc_y, wc_x))
d1 = .75 # length along z axis from base joint to J1
L1 = 1.25 # Length along x2 axis from J2 to J3
L2 = 1.5 # length along x3 axis from J3 to wc (joint 5)
a1 = .35 # distance along x axis between J1 and J2
r = sqrt((wc_x - a1 * cos(theta1))**2 +
(wc_y - a1 * sin(theta1))**2 + (wc_z - d1)**
2) #magnitude of vector from z - d1 axis to wrist center
cosAlpha = (L2**2 - L1**2 - r**2) / (-2 * L1 * r) #cosine of alpha
# In the equation below, sqrt(1+cosAlpha^2) works as well
theta2 = float(pi / 2 - atan2((wc_z - d1),
sqrt((wc_x - a1 * cos(theta1))**2 +
(wc_y - a1 * sin(theta1))**2)) -
atan2(sqrt(1 - cosAlpha**2), cosAlpha))
cosBeta = (r**2 - L1**2 - L2**2) / (-2 * L1 * L2)
# here we also must subtract the offset from theta3 due to the fact that J4 is slightly lower on the z-axis
theta3 = float(pi / 2 - atan2(sqrt(1 - cosBeta**2), cosBeta) -
atan2(.056, 1.5))
#once the q1-q3 vales are known, we can derive the orientation
# first, get the transformation matrix from the 3rd joint to the 6th joint to solve for q4-q6
R0_3_inv = Matrix([[
sin(theta2 + theta3) * cos(theta1),
sin(theta1) * sin(theta2 + theta3),
cos(theta2 + theta3)
],
[
cos(theta1) * cos(theta2 + theta3),
sin(theta1) * cos(theta2 + theta3),
-sin(theta2 + theta3)
], [-sin(theta1), cos(theta1), 0]])
R3_6 = R0_3_inv * R_corr
# from here, we can just plug and chug to solve for the remaining angles
theta6 = float(
atan2((-R3_6[4] / sqrt(1 - R3_6[5]**2)),
(R3_6[3] / sqrt(1 - R3_6[5]**2))))
theta4 = float(
atan2((R3_6[8] / sqrt(1 - R3_6[5]**2)),
(-R3_6[2] / sqrt(1 - R3_6[5]**2))))
theta5 = float(atan2((R3_6[3] / cos(theta6)), R3_6[5]))
# Populate response for the IK request
joint_trajectory_point.positions = [
theta1, theta2, theta3, theta4, theta5, theta6
]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
# Initialize service response
joint_trajectory_list = []
joint_trajectory_errors = []
for x in xrange(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
p[0] = req.poses[x].position.x
p[1] = req.poses[x].position.y
p[2] = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
# Compensate for rotation discrepancy between DH parameters and Gazebo
Rrpy = Rrpy_pre.evalf(subs={rpy1: roll, rpy2: pitch, rpy3: yaw})
#calculate wrist center
wc = p - 0.303 * Rrpy[:, 2]
# Calculate joint angles using Geometric IK method
theta1 = atan2(wc[1], wc[0])
# Use Law of Cosines to find required angles to calculate theta2 and theta3; A=1.5, C=1.25
# Since wrist center (wc) based on global frame, need to subtract relative x & z coords of joint 2
B = sqrt(
pow(sqrt(wc[0] * wc[0] + wc[1] * wc[1]) - 0.35, 2) +
pow((wc[2] - 0.75), 2))
angle_a = acos((pow(B, 2) - 0.6875) / (2.5 * B))
angle_b = acos((3.8125 - pow(B, 2)) / (3.75))
angle_c = acos((0.6875 + pow(B, 2)) / (3 * B))
# Equations to calculate angles theta2 and theta3 based on above results
theta2 = pi / 2 - angle_a - atan2(
(wc[2] - 0.75), (sqrt(wc[0] * wc[0] + wc[1] * wc[1]) - 0.35))
theta3 = pi / 2 - (
angle_b + 0.036
) # 0.036 angle accounts for slight sag between joint 3 to joint 4
# Evaluate rotation matrix from joint 3 to 6; result contains no variables
R3_6 = R0_3inv.evalf(subs={
q1: theta1,
q2: theta2,
q3: theta3
}) * Rrpy
# Based on equations derived from rotation matrix extracted earlier from DH convention
theta4 = atan2(R3_6[2, 2], -R3_6[0, 2])
theta5 = atan2(sqrt(pow(R3_6[0, 2], 2) + pow(R3_6[2, 2], 2)),
R3_6[1, 2])
theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])
# Uncomment the below to calculate magnitude of position error
#FK = T0_G.evalf(subs={q1: theta1, q2: theta2, q3: theta3, q4: theta4, q5: theta5, q6: theta6})
#your_ee = [FK[0,3],FK[1,3],FK[2,3]]
#error = sqrt(pow(p[0]-your_ee[0],2) + pow(p[1]-your_ee[1],2) + pow(p[2]-your_ee[2],2))
# Populate response for the IK request
joint_trajectory_point.positions = [
theta1, theta2, theta3, theta4, theta5, theta6
]
joint_trajectory_list.append(joint_trajectory_point)
#joint_trajectory_errors.append(error) # Uncomment to create list of error magnitudes
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
# Uncomment below to plot magnitude of errors of requested pose before executing motion; must exit before continuing
#plt.plot(joint_trajectory_errors)
#plt.ylabel('Error, m')
#plt.xlabel('Pose')
#plt.title('Inverse Kinematics Errors')
#plt.show()
return CalculateIKResponse(joint_trajectory_list)
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
# Initialize service response
joint_trajectory_list = []
######### Start code to open json error file ###########
# open the saved error list. if file does not exist, create it
# load the json from the error list
# open error list from disk
error_list_path = "./error_list.json"
if os.path.isfile(error_list_path):
with open(error_list_path, "r") as error_list_file:
json_error_list = json.loads(error_list_file.read(),
encoding='utf-8')
px_error_list = json_error_list["px"]
py_error_list = json_error_list["py"]
pz_error_list = json_error_list["pz"]
roll_error_list = json_error_list["roll"]
pitch_error_list = json_error_list["pitch"]
yaw_error_list = json_error_list["yaw"]
else:
# if file does not exist, create an empty file and initialize lists
with open(error_list_path, "w") as error_list_file:
json_error_list = {
"px": [],
"py": [],
"pz": [],
"roll": [],
"pitch": [],
"yaw": []
}
px_error_list = json_error_list["px"]
py_error_list = json_error_list["py"]
pz_error_list = json_error_list["pz"]
roll_error_list = json_error_list["roll"]
pitch_error_list = json_error_list["pitch"]
yaw_error_list = json_error_list["yaw"]
json.dump(json_error_list, error_list_file)
######### End code to open json error file ###########
for x in xrange(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
# create Rrpy matrix
Rrpy = generate_rrpy_matrix(roll, pitch, yaw)
dist_wrist_to_effector = s[d7]
d6_val = s[d6]
# compute wrist values
wx, wy, wz = get_wrist_coordinates(Rrpy, px, py, pz,
d6_val + dist_wrist_to_effector)
# Calculate joint angles using Geometric IK method
# compute theta for joint 1
theta1 = atan2(wy, wx)
theta1 = theta1.evalf()
# compute the side adjacents of theta 3
distance_joint_2to3 = s[a2]
distance_joint_3to5 = sqrt((s[a3]**2) + (s[d4]**2))
# get the offsets to the origin of joint 2
x_offset_to_joint2 = s[a1]
z_offset_to_joint2 = s[d1]
# compute for the elbow up value of theta 3
unadjusted_theta_3 = get_theta_3(distance_joint_2to3,
distance_joint_3to5, wx, wz,
theta1, x_offset_to_joint2,
z_offset_to_joint2)
# choose the first of the two theta_3 results, which is the elbow up result
theta3 = unadjusted_theta_3.evalf() - RADS_AT_REST_JOINT3
# compute the parts used for theta 2
alpha = get_alpha(distance_joint_2to3,
distance_joint_3to5,
wx,
wz,
theta1,
joint_2_x_offset=s[a1],
joint_2_z_offset=s[d1])
beta = get_beta(wx, wz, theta1, s[a1], s[d1])
# compute for theta2 without the offset
unadjusted_theta_2 = get_theta_2(alpha, beta).evalf()
# compute theta 2 value
theta2 = RADS_AT_REST_JOINT2 - unadjusted_theta_2
theta2 = theta2.evalf()
# get the matrix for base link to joint 3, using computed theta1, 2, and 3 values
R0_3 = T0_3[0:3, 0:3]
R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
# convert sympy matrix to numpy matrix to avoid errors
# how to convert lifted from: https://stackoverflow.com/a/37491889
R0_3 = np.array(R0_3).astype(np.float64)
# correct the orientation of the Rrpy matrix
Rrpy = Rrpy * rotate_y(pi / 2)[0:3, 0:3] * rotate_z(pi)[0:3, 0:3]
# get the matrix values for the wrist
R3_6 = (np.linalg.inv(R0_3)) * Rrpy
# compute values for thetas 4, 5, and 6
theta4 = atan2(R3_6[2, 2], -R3_6[0, 2])
theta4 = theta4.evalf()
theta5 = acos(R3_6[1, 2])
theta5 = theta5.evalf()
theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])
theta6 = theta6.evalf()
# compute forward kinematics for error checking
FK_matrix = T_total.evalf(
subs={
q1: theta1,
q2: theta2,
q3: theta3,
q4: theta4,
q5: theta5,
q6: theta6
})
FK_px = FK_matrix[0, 3]
FK_py = FK_matrix[1, 3]
FK_pz = FK_matrix[2, 3]
FK_roll = get_roll(FK_matrix)
FK_pitch = get_pitch(FK_matrix)
FK_yaw = get_yaw(FK_matrix)
# compute the error values for the px, py, pz and rpy
px_error = (px - FK_px) # ** 2
py_error = (py - FK_py) # ** 2
pz_error = (pz - FK_pz) # ** 2
roll_error = (roll - FK_roll) # ** 2
pitch_error = (pitch - FK_pitch) # ** 2
yaw_error = (yaw - FK_yaw) # ** 2
# append each of the error values to their respective lists
px_error_list.append(float(px_error))
py_error_list.append(float(py_error))
pz_error_list.append(float(pz_error))
roll_error_list.append(float(roll_error))
pitch_error_list.append(float(pitch_error))
yaw_error_list.append(float(yaw_error))
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
joint_trajectory_point.positions = [
theta1, theta2, theta3, theta4, theta5, theta6
]
joint_trajectory_list.append(joint_trajectory_point)
# once once done with IK and FK operations, save the updated set of errors to disk
with open(error_list_path, "w") as error_list_file:
json.dump(json_error_list, error_list_file, encoding='utf-8')
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
goal_pose2 = Pose() goal_pose2.position.x = 0.4 goal_pose2.position.y = 0.3 goal_pose2.position.z = 0.2 goal_pose2.orientation.x = 0.0 goal_pose2.orientation.y = 0.0 goal_pose2.orientation.z = 1.0 goal_pose2.orientation.w = 0.0 test_parray = PoseArray() test_parray.header = header test_parray.poses.append(goal_pose) test_parray.poses.append(goal_pose1) test_parray.poses.append(goal_pose2) jt_point1 = JointTrajectoryPoint() jt_point1.positions = [0.0, 1.0, 2.0] jt_point1.time_from_start.secs = 0 jt_point1.time_from_start.nsecs = 00000000 jt_point2 = JointTrajectoryPoint() jt_point2.positions = [1.0, 2.0, 3.0] jt_point2.time_from_start.secs = 0 jt_point2.time_from_start.nsecs = 10000000 jt_point3 = JointTrajectoryPoint() jt_point3.positions = [2.0, 3.0, 4.0] jt_point3.time_from_start.secs = 0 jt_point3.time_from_start.nsecs = 20000000 test_traj = RobotTrajectory()
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
### FK
# Create symbols
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') # theta_i
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols('alpha0:7')
# Create Modified DH parameters
DH_table = { alpha0: 0, a0: 0, d1: 0.75,
alpha1: radians(-90), a1: 0.35, d2: 0, q2: q2-radians(90),
alpha2: 0, a2: 1.25, d3: 0,
alpha3: radians(-90), a3: -0.054, d4: 1.50,
alpha4: radians(90), a4: 0, d5: 0,
alpha5: radians(-90), a5: 0, d6: 0,
alpha6: 0, a6: 0, d7: 0.303}
# Create individual transformation matrices
T0_1 = DH_TF_Matrix(alpha0, a0, d1, q1).subs(DH_table)
T1_2 = DH_TF_Matrix(alpha1, a1, d2, q2).subs(DH_table)
T2_3 = DH_TF_Matrix(alpha2, a2, d3, q3).subs(DH_table)
T3_4 = DH_TF_Matrix(alpha3, a3, d4, q4).subs(DH_table)
T4_5 = DH_TF_Matrix(alpha4, a4, d5, q5).subs(DH_table)
T5_6 = DH_TF_Matrix(alpha5, a5, d6, q6).subs(DH_table)
T6_EE = DH_TF_Matrix(alpha6, a6, d7, q7).subs(DH_table)
T0_2 = (T0_1 * T1_2)
T0_3 = (T0_2 * T2_3)
T0_4 = (T0_3 * T3_4)
T0_5 = (T0_4 * T4_5)
T0_6 = (T0_5 * T5_6)
T0_EE = (T0_6 * T6_EE)
# Correction Needed to Account of Orientation Difference Between Definition of
# Gripper Link in URDF versus DH Convention
r, p, y = symbols('r p y')
R_corr = rot_z(y).subs(y, radians(180))*rot_y(p).subs(p, radians(-90))
R_EE = rot_z(y)*rot_y(p)*rot_x(r)
R_EE = R_EE * R_corr
###
# Initialize service response
joint_trajectory_list = []
for x in xrange(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(
[req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w])
### IK code here
# Compensate for rotation discrepancy between DH parameters and Gazebo
R_EE = R_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
EE = Matrix([[px],
[py],
[pz]])
WC = EE - DH_table[d7] * R_EE[:, 2]
# Calculate joint angles using Geometric IK method
theta1 = atan2(WC[1], WC[0])
# SSS triangle for theta2 and theta3
side_a = 1.50097169 # sqrt(DH_table[d4]*DH_table[d4] + DH_table[a3]*DH_table[a3])
side_a_squared = 2.252916
r = sqrt(WC[0]*WC[0] + WC[1]*WC[1]) - DH_table[a1]
s = WC[2] - DH_table[d1]
side_b = sqrt(r*r + s*s)
side_c = 1.25 # DH_table[a2]
side_c_squared = 1.5625
two_side_a_times_c = 3.75242923 # 2*side_c*side_a
cos_angle_a = np.clip((side_b*side_b + side_c_squared - side_a_squared)/(2*side_b*side_c), -1, 1)
cos_angle_b = np.clip((side_c_squared + side_a_squared - side_b*side_b)/(two_side_a_times_c), -1, 1)
angle_a = acos(cos_angle_a)
angle_b = acos(cos_angle_b)
theta2 = radians(90) - angle_a - atan2(s, r)
theta3 = radians(90) - angle_b - atan2(DH_table[a3], DH_table[d4])
# Inverse Orientation
R0_3 = T0_1[0:3,0:3] * T1_2[0:3,0:3] * T2_3[0:3,0:3]
R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3:theta3})
R3_6 = R0_3.transpose()*R_EE
theta5 = atan2(sqrt(R3_6[0,2]**2 + R3_6[2,2]**2), R3_6[1,2])
if (theta5 > pi) :
theta4 = atan2(-R3_6[2,2], R3_6[0,2])
theta6 = atan2(R3_6[1,1],-R3_6[1,0])
else:
theta4 = atan2(R3_6[2,2], -R3_6[0,2])
theta6 = atan2(-R3_6[1,1],R3_6[1,0])
# Angle limits
theta1 = clip_angle(theta1, -185, 185)
theta2 = clip_angle(theta2, -45, 85)
theta3 = clip_angle(theta3, -210, 65)
theta4 = clip_angle(theta4, -350, 350)
theta5 = clip_angle(theta5, -125, 125)
theta6 = clip_angle(theta6, -350, 350)
# ###
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
joint_trajectory_point.positions = [theta1, theta2, theta3, theta4, theta5, theta6]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
import math
from std_msgs.msg import String, Header
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
rospy.init_node('dynamixel_workbench')
mypub = rospy.Publisher('/dynamixel_workbench/joint_trajectory', JointTrajectory, queue_size=100)
jt_msg = JointTrajectory()
count = 0
while True:
jt_msg = JointTrajectory()
jt_msg.header.stamp = rospy.Time.now()
jt_msg.joint_names = [ "pan_joint", "tilt_joint", "yaw_joint" ]
# create a JTP instance and configure it
for i in range(3):
jtp_msg = JointTrajectoryPoint()
jtp_msg.velocities = [0.1]
# setup the rest of the pt
jtp_msg.time_from_start = rospy.Duration.from_sec(60.1)
jt_msg.points.append(jtp_msg)
mypub.publish(jt_msg)
count += 1
if count > 1000000:
break
def handle_calculate_IK(req):
'''A function that calculates angles of kuka arm joints required to reach poses defined as end effector positon and orientation in quaternions '''
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
### Your FK code here
# Create symbols
y, p, r = symbols('y p r')
q1, q2, q3, q4, q5, q6 = symbols('q1:7')
#
#
# Create Modified DH parameters
#
#
# Define Modified DH Transformation matrix
#
#
# Create individual transformation matrices
# yaw pitch roll rotation from euler angles enables to derive orientation matrix
Rrpy_gaz = Matrix([
[cos(p)*cos(y), sin(p)*sin(r)*cos(y) - sin(y)*cos(r), sin(p)*cos(r)*cos(y) + sin(r)*sin(y)],
[sin(y)*cos(p), sin(p)*sin(r)*sin(y) + cos(r)*cos(y), sin(p)*sin(y)*cos(r) - sin(r)*cos(y)],
[ -sin(p), sin(r)*cos(p), cos(p)*cos(r)]])
# Rotation matrix for first 3 joints
R0_3 = Matrix([
[sin(q2 + q3)*cos(q1), cos(q1)*cos(q2 + q3), -sin(q1)],
[sin(q1)*sin(q2 + q3), sin(q1)*cos(q2 + q3), cos(q1)],
[ cos(q2 + q3), -sin(q2 + q3), 0]])
# Matrix required to change from ROS to DH notation
R_corr_inv = Matrix([
[0, 0, 1],
[0, -1, 0],
[1., 0, 0]])
# Initialize service response
joint_trajectory_list = []
for x in xrange(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(
[req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w])
end_pose = Matrix([px,py,pz])
### IK code
## Calculate writst center position by moving 0.303 units along x axis of gazebo's gripper link
Rrpy_gaz_eval = Rrpy_gaz.evalf(subs={y: yaw, p: pitch, r:roll}) # get rotation matrix from euler angles
L_xyz = Rrpy_gaz_eval[0:3,0] # get firs collumn representing x direction of the girpper link in ROS
Wrc = simplify(end_pose - L_xyz*0.303) #wrist center
Wrc2 = Matrix([ sqrt(Wrc[0]**2+Wrc[1]**2)-0.35, 0, Wrc[2]-0.75]) #wrist center with respect to joint 2
## Calculate joints 1-3 values
theta1 = atan2(Wrc[1], Wrc[0])
# Calculate leghts of sids of triangle representing joint 2 and 4
lenA = sqrt(1.5**2 + 0.054**2)
lenB = sqrt(Wrc2[0]**2 + Wrc2[2]**2)
lenC = 1.25
# calculate angles from cosine law
alph = acos( (lenB**2 + lenC**2 - lenA**2) / (2*lenB*lenC))
beta = acos( (lenA**2 + lenC**2 - lenB**2) / (2*lenA*lenC))
epsi = atan(Wrc2[2]/Wrc2[0])
delt = atan(0.054/1.5)
# derive joint angles
theta2 = pi/2-epsi-alph
theta3 = pi/2 - beta - delt
## Calculate joionts 4-6
R0_3_eval = R0_3.evalf(subs={q1:theta1, q2:theta2, q3:theta3}) # evaluate matrix representig transition from base link to link 3 using previously derived joint angles
R3_6 = R0_3_eval.inverse_ADJ() * Rrpy_gaz_eval * R_corr_inv # compute matrxi representing transition from link 3 to gripper link in modified DH notation
# comput joint angles
theta4 = atan2(R3_6[2,2], -R3_6[0,2])
theta5 = atan2(sqrt(R3_6[0,2]**2 + R3_6[2,2]**2), R3_6[1,2])
theta6 = atan2(-R3_6[1,1], R3_6[1,0])
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
joint_trajectory_point.positions = [theta1, theta2, theta3, theta4, theta5, theta6]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
def add_point(self, positions, time):
point = JointTrajectoryPoint()
point.positions = copy(positions)
point.velocities = [0.0] * len(self._goal.trajectory.joint_names)
point.time_from_start = rospy.Duration(time)
self._goal.trajectory.points.append(point)
def __init__(self):
# initialize node
rospy.init_node('ur5e_control', anonymous=True)
# laser_projector = LaserProjection()
pointlist = []
iteration = -1
old = 0
counter = 0
delta = 0
startTime = time.time()
prevtime = time.time()
# move_it_turtlebot = MoveGroupPythonIntefaceTutorial()
# set node rate
loop_rate = 120 #500
ts = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
t_prev = time.time()
keypress = time.time()
i = 0
sigfig = 3
# DH Parameters
d1 = 0.1625
a2 = 0.42500
a3 = 0.3922
d4 = 0.1333
d5 = 0.0997
d6 = 0.0996
self.alpha = [0, pi / 2, 0, 0, pi / 2, -pi / 2]
self.a = [0, 0, -a2, -a3, 0, 0]
self.d = [d1, 0, 0, d4, d5, d6]
self.theta = np.zeros((6, 1)) + 0.00001
self.Ro = np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]])
self.Po = np.array([[-a2 - a3], [-d4 - d6], [d1 - d5]])
self.wlist = np.array([
[[0], [0], [1]],
[[0], [-1], [0]],
[[0], [-1], [0]],
[[0], [-1], [0]],
[[0], [0], [-1]],
[[0], [-1], [0]],
])
self.Qlist = np.array([
[[0], [0], [d1]],
[[0], [0], [d1]],
[[-a2], [0], [d1]],
[[-a2 - a3], [0], [d1]],
[[-a2 - a3], [-d4], [0]],
[[-a2 - a3], [0], [d1 - d5]],
])
self.J = np.array([[1], [1], [1], [1], [1], [1]])
self.jacobian = eye(6)
self.qv = []
joint_traj = JointTrajectory()
joint_point = JointTrajectoryPoint()
joint_vels = Float64MultiArray()
self.joint_vels = Float64MultiArray()
self.hand_state = Float64MultiArray()
self.command_pub = rospy.Publisher(
'/pos_based_pos_traj_controller/command',
JointTrajectory,
queue_size=1)
# command_pub2 = rospy.Publisher('/ur_driver/jointspeed',JointTrajectory,queue_size =1)
self.command_pub3 = rospy.Publisher(
'joint_group_vel_controller/command',
Float64MultiArray,
queue_size=1)
self.desiredvelpub = rospy.Publisher(
'joint_group_vel_controller/joint_vel',
Float64MultiArray,
queue_size=1)
self.hand_state_pub = rospy.Publisher('hand_state',
Float64MultiArray,
queue_size=1)
self.robot_call_pub = rospy.Publisher('robot_view',
Vector3,
queue_size=1)
subname = rospy.Subscriber('joint_states', JointState,
self.callback_function)
rospy.on_shutdown(self.shutdown)
joint_traj.joint_names = [
'elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
joint_point.positions = np.array([
-0.9774951934814453, -1.6556574306883753, -1.6917484442340296,
-0.17756159723315434, 1.5352659225463867, 2.1276955604553223
])
# joint_point.positions = np.array([0,0,0,0,0,0])
joint_point.velocities = np.array([1, -1, -1, -1, -1, -1])
joint_point.accelerations = np.array([1, -1, -1, -1, -1, -1])
# joint_point.velocities = np.array([-0.19644730172630054, -0.1178443083991266, 0.14913797608558607, 0.41509166700461164, 0.14835661279488965, -0.10175914445443687])
# joint_point.accelerations = np.array([-1.4413459458952043, -0.8646309451201031, 1.0942345113473044, 3.0455531135039218, 1.0885016007834076, -0.7466131070688594])
joint_point.time_from_start.secs = 10
joint_vels.data = np.array([0, 0, 0, 0, 0, 0])
ready = 0
joint_traj.points = [joint_point]
self.jacob_pseudo_inv = np.zeros(6)
cart_vel_des = np.array([0, 0, 0, 0, 0, 0])
self.body_frame = False
self.prev_time = time.time()
self.joint_vel_des = np.array([0, 0, 0, 0, 0, 0])
self.home = np.array(
[0, -70 * pi / 180, -140 * pi / 180, -60 * pi / 180, 0, 0])
self.cupup = np.array([
0, -70 * pi / 180, -140 * pi / 180, -60 * pi / 180, -90 * pi / 180,
0
])
self.movehome = False
self.count = 0
self.Tstart = []
self.Toffset = np.concatenate(
(np.concatenate((eye(3), np.array([[0], [-0.037338], [0.135142]])),
axis=1), np.array([[0, 0, 0, 1]])),
axis=0)
while not rospy.is_shutdown():
# command_pub.publish(joint_traj)
f = 2
w = 2 * 3.14159 * f
current_time = time.time() - startTime
if current_time > 2:
ready += 1
if ready == 1:
startTime = time.time()
print(startTime)
ready += 1
elif ready > 1:
# pass
# print(current_time)
# print(current_time, self.get_vel_from_data(current_time))
[x, z] = self.get_vel_from_data(current_time)
if self.body_frame:
cart_vel_des = np.array([z, -x, 0, 0, 0, 0])
else:
cart_vel_des = np.array([x, 0, z, 0, 0, 0])
self.joint_vel_des = np.matmul(self.jacob_pseudo_inv,
cart_vel_des)
joint_vels.data = self.joint_vel_des
# j2 = self.joint_vel_des[2]
# joint_vels.data = np.array([0,0,j2,0,0,0])
# print(self.qv)
# print(around(joint_vels.data-self.qv ,4))
# print(around(joint_vels.data ,4),around(self.v ,4))
# raw_input()
if current_time > 1:
self.movehome = True
if self.movehome:
joint_vels.data = np.array([0, 0, 0, 0, 0, 0])
# print(joint_vels.data )
##### [1,2,3,5]
# plt.plot(self.joint_vel_des[1], 'ro',self.qv[1],'bs')
# plt.axis([0, 6, 0, 20])
# plt.show()
# print(current_time, joint_vels.data)
self.command_pub3.publish(joint_vels)
# joint_vels.data = np.array([0,0,0.4*sin(2*current_time), 0.4*sin(2*current_time),0,0.4*sin(2*current_time)])
# joint_vels.data = np.array([0,0,0,0,0,0])
# joint_vels.data = np.array([0,0,-1*sin(2*current_time),1*sin(2*current_time),0,0])
# cart_vel_des = np.matmul(self.jacobian,joint_vels.data.transpose())
# cart_vel_des = np.array([0.3*sin(2*current_time),0.3*sin(2*current_time+pi/2),0,0,0,0])
# cart_vel_des = np.array([0,0,0.1*sin(2*current_time),0,0,0])
# cart_vel_des = np.array([0.1*sin(2*current_time),0,0,0,0,0])
# print(around(cart_vel_des,4))
# print(around(joint_vels.data ,4))
# lagrangemult = 0.0001
# joint_vel_des = np.matmul(self.jacob_pseudo_inv,cart_vel_des)
# joint_vels.data = joint_vel_des
# print("failed")
# print(current_time,ready)
############################################################################
# self.command_pub3.publish(joint_vels)
############################################################################
# joint_point.positions = np.array([cos(3*current_time), sin(3*current_time)-1, -0.21748375933108388, 1.4684332653952596, -0.2202624218007605, 0.08156436078884344])
# # joint_point.velocities = np.array([cos(3*current_time), sin(3*current_time)-1, -0.21748375933108388, 1.4684332653952596, -0.2202624218007605, 0.08156436078884344])
# joint_traj.points = [joint_point]
# command_pub2.publish(joint_traj)
# a = raw_input("============ Moving arm to pose 1")
# s1 = move_it_turtlebot.go_to_pose_goal(np.round([0.4,-0.04,.7,0,pi/2,0],2))
# print(move_it_turtlebot.move_group.get_current_pose())
# a = raw_input("============ Moving arm to pose 2")
# s1 = move_it_turtlebot.go_to_pose_goal(np.round([0.4,-0.04,.6,0,pi/2,0],2))
# print(move_it_turtlebot.move_group.get_current_pose())
# a = raw_input("aaaaaa")
rate.sleep()
class HelloNode:
def __init__(self):
self.joint_state = None
self.point_cloud = None
def joint_states_callback(self, joint_state):
self.joint_state = joint_state
def point_cloud_callback(self, point_cloud):
self.point_cloud = point_cloud
def move_to_pose(self, pose, async=False, custom_contact_thresholds=False):
joint_names = [key for key in pose]
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration(0.0)
trajectory_goal = FollowJointTrajectoryGoal()
trajectory_goal.goal_time_tolerance = rospy.Time(1.0)
trajectory_goal.trajectory.joint_names = joint_names
if not custom_contact_thresholds:
joint_positions = [pose[key] for key in joint_names]
point.positions = joint_positions
trajectory_goal.trajectory.points = [point]
else:
pose_correct = all([len(pose[key]) == 2 for key in joint_names])
if not pose_correct:
rospy.logerr(
"HelloNode.move_to_pose: Not sending trajectory due to improper pose. custom_contact_thresholds requires 2 values (pose_target, contact_threshold_effort) for each joint name, but pose = {0}"
.format(pose))
def or_to_ros_trajectory(robot, traj, time_tolerance=0.01):
""" Convert an OpenRAVE trajectory to a ROS trajectory.
@param robot: OpenRAVE robot
@type robot: openravepy.Robot
@param traj: input trajectory
@type traj: openravepy.Trajectory
@param time_tolerance: minimum time between two waypoints
@type time_tolerance: float
"""
import numpy
from rospy import Duration
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
assert time_tolerance >= 0.
if traj.GetEnv() != robot.GetEnv():
raise ValueError(
'Robot and trajectory are not in the same environment.')
cspec = traj.GetConfigurationSpecification()
dof_indices, _ = cspec.ExtractUsedIndices(robot)
traj_msg = JointTrajectory(joint_names=[
robot.GetJointFromDOFIndex(dof_index).GetName()
for dof_index in dof_indices
])
time_from_start = 0.
prev_time_from_start = 0.
# this hack was added by Travers Rhodes to prevent 360 degree spins we were seeing
# when running the trajectory generated by MoveToHandPosition
# if a joint changes by more than pi to the new joint location, we reindex
# so the joint takes a shorter path
previous_q = None
for iwaypoint in xrange(traj.GetNumWaypoints()):
waypoint = traj.GetWaypoint(iwaypoint)
dt = cspec.ExtractDeltaTime(waypoint)
q = cspec.ExtractJointValues(waypoint, robot, dof_indices, 0)
qd = cspec.ExtractJointValues(waypoint, robot, dof_indices, 1)
qdd = cspec.ExtractJointValues(waypoint, robot, dof_indices, 2)
#re-index each joint index so that it is no more than pi away from the previous
# j
if previous_q is not None:
for jointIndx, _ in enumerate(q):
while q[jointIndx] - previous_q[jointIndx] > numpy.pi:
q[jointIndx] -= 2 * numpy.pi
while previous_q[jointIndx] - q[jointIndx] > numpy.pi:
q[jointIndx] += 2 * numpy.pi
previous_q = q
if dt is None:
raise ValueError('Trajectory is not timed.')
elif q is None:
raise ValueError('Trajectory does not contain joint values')
elif qdd is not None and qd is None:
raise ValueError('Trajectory contains accelerations,'
' but not velocities.')
# Duplicate waypoints break trajectory execution, so we explicitly
# filter them out. Note that we check the difference in time between
# the current and the previous waypoint, not the raw "dt" value. This
# is necessary to support very densely sampled trajectories.
time_from_start += dt
deltatime = time_from_start - prev_time_from_start
# openrave includes the first trajectory point as current, with time zero
# ros ignores this. so if time zero, then skip
if time_from_start == 0:
continue
if iwaypoint > 0 and deltatime < time_tolerance:
logger.warning(
'Skipped waypoint %d because deltatime is %.3f < %.3f.',
iwaypoint, deltatime, time_tolerance)
continue
prev_time_from_start = time_from_start
# Create the waypoint.
traj_msg.points.append(
JointTrajectoryPoint(
positions=list(q),
velocities=list(qd) if qd is not None else [],
accelerations=list(qdd) if qdd is not None else [],
time_from_start=Duration.from_sec(time_from_start)))
assert abs(time_from_start - traj.GetDuration()) < time_tolerance
return traj_msg
joint_names = [
"_shoulder_pan_joint", "_shoulder_lift_joint", "_upper_arm_roll_joint",
"_elbow_flex_joint", "_forearm_roll_joint", "_wrist_flex_joint",
"_wrist_roll_joint"
]
joint_names1 = [whicharm + x for x in joint_names]
goal = JointTrajectoryGoal()
goal.trajectory.header.stamp = rospy.get_rostime()
goal.trajectory.joint_names = joint_names1
jointangles = [0] * 7
jointangles[0] = .7
#jointangles[1]=-.3
jointvelocities = [0] * 7
jointaccelerations = [0] * 7
time_for_motion = rospy.Duration(1.0)
point = JointTrajectoryPoint(jointangles, jointvelocities, jointaccelerations,
time_for_motion)
goal.trajectory.points = [
point,
]
joint_action_client.send_goal(goal)
time.sleep(1.0)
point = JointTrajectoryPoint([0] * 7, [0] * 7, [0] * 7, rospy.Duration(3.0))
goal.trajectory.points = [
point,
]
joint_action_client.send_goal(goal)
def __init__(self):
rospy.init_node('doublearm_trajectory_demo')
# 是否需要回到初始化的位置
reset = rospy.get_param('~reset', False)
# 机械臂中joint的命名
doublearm_joints = [
'left_joint2', 'left_joint3', 'left_joint4', 'left_joint5',
'left_joint6', 'left_palm_joint', 'left_thumb_joint',
'left_index_finger_joint', 'left_middle_finger_joint',
'left_third_finger_joint', 'left_little_finger_joint',
'right_joint2', 'right_joint3', 'right_joint4', 'right_joint5',
'right_joint6', 'right_palm_joint', 'right_thumb_joint',
'right_index_finger_joint', 'right_middle_finger_joint',
'right_third_finger_joint', 'right_little_finger_joint'
]
if reset:
# 如果需要回到初始化位置，需要将目标位置设置为初始化位置的六轴角度
doublearm_goal = [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0
]
else:
# 如果不需要回初始化位置，则设置目标位置的六轴角度
doublearm_goal = [
1, 0.25, 0.48, 0.8, 0.4, 0.25, 0.25, 0.25, 0.25, 0.25, 0.25, 1,
0.25, 0.48, 0.8, 0.4, 0.25, 0.25, 0.25, 0.25, 0.25, 0.25
]
# 连接机械臂轨迹规划的trajectory action server
rospy.loginfo('Waiting for doublearm trajectory controller...')
doublearm_client = actionlib.SimpleActionClient(
'doublearm_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
doublearm_client.wait_for_server()
rospy.loginfo('...connected.')
# 使用设置的目标位置创建一条轨迹数据
doublearm_trajectory = JointTrajectory()
doublearm_trajectory.joint_names = doublearm_joints
doublearm_trajectory.points.append(JointTrajectoryPoint())
doublearm_trajectory.points[0].positions = doublearm_goal
doublearm_trajectory.points[0].velocities = [
0.0 for i in doublearm_joints
]
doublearm_trajectory.points[0].accelerations = [
0.0 for i in doublearm_joints
]
doublearm_trajectory.points[0].time_from_start = rospy.Duration(3.0)
rospy.loginfo('Moving the doublearm to goal position...')
# 创建一个轨迹目标的空对象
doublearm_goal = FollowJointTrajectoryGoal()
# 将之前创建好的轨迹数据加入轨迹目标对象中
doublearm_goal.trajectory = doublearm_trajectory
# 设置执行时间的允许误差值
doublearm_goal.goal_time_tolerance = rospy.Duration(0.0)
# 将轨迹目标发送到action server进行处理，实现机械臂的运动控制
doublearm_client.send_goal(doublearm_goal)
# 等待机械臂运动结束
doublearm_client.wait_for_result(rospy.Duration(5.0))
rospy.loginfo('...done')
def add_point(self, positions, time):
point = JointTrajectoryPoint()
point.positions = copy(positions)
point.time_from_start = rospy.Duration(time)
self._goal.trajectory.points.append(point)
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
### Your FK code here
# Create symbols
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
b0, b1, b2, b3, b4, b5, b6 = symbols('b0:7')
# Create Modified DH parameters
s = {
b0: 0,
a0: 0,
d1: 0.75,
q1: q1,
b1: -pi / 2.0,
a1: 0.35,
d2: 0,
q2: q2 - pi / 2.0,
b2: 0,
a2: 1.25,
d3: 0,
q3: q3,
b3: -pi / 2.0,
a3: -0.054,
d4: 1.5,
q4: q4,
b4: pi / 2.0,
a4: 0,
d5: 0,
q5: q5,
b5: -pi / 2.0,
a5: 0,
d6: 0,
q6: q6,
b6: 0,
a6: 0,
d7: 0.303,
q7: 0
}
# Define Modified DH Transformation matrix
def DH_T(ap, a, th, d):
R_x = Matrix([[1, 0, 0, 0], [0, cos(ap), -sin(ap), 0],
[0, sin(ap), cos(ap), 0], [0, 0, 0, 1]])
R_z = Matrix([[cos(th), -sin(th), 0, 0], [sin(th),
cos(th), 0, 0],
[0, 0, 1, 0], [0, 0, 0, 1]])
TX = Matrix([[1, 0, 0, a], [0, 1, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
TZ = Matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, d],
[0, 0, 0, 1]])
DH_T = Matrix(R_x * TX * R_z * TZ)
return DH_T
# Create individual transformation matrices
T0_1 = DH_T(b0, a0, q1, d1).subs(s)
T1_2 = DH_T(b1, a1, q2, d2).subs(s)
T2_3 = DH_T(b2, a2, q3, d3).subs(s)
T3_4 = DH_T(b3, a3, q4, d4).subs(s)
T4_5 = DH_T(b4, a4, q5, d5).subs(s)
T5_6 = DH_T(b5, a5, q6, d6).subs(s)
T6_EE = DH_T(b6, a6, q7, d7).subs(s)
T0_EE = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_EE
# Extract rotation matrices from the transformation matrices
# Initialize service response
joint_trajectory_list = []
for x in xrange(0, len(req.poses)):
#print("pose",req.poses[x])
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
### Your IK code here
# Compensate for rotation discrepancy between DH parameters and Gazebo
R_z = Matrix([[cos(pi), -sin(pi), 0], [sin(pi),
cos(pi), 0], [0, 0, 1]])
R_y = Matrix([[cos(-pi / 2.0), 0,
sin(-pi / 2.0)], [0, 1, 0],
[-sin(-pi / 2.0), 0,
cos(-pi / 2.0)]])
R_corr = R_z * R_y
#find EE rotation matrix
r, p, y = symbols('r p y')
RX = Matrix([[1.0, 0, 0], [0, cos(r), -sin(r)],
[0, sin(r), cos(r)]])
RZ = Matrix([[cos(p), -sin(p), 0], [sin(p), cos(p), 0], [0, 0, 1]])
RY = Matrix([[cos(y), 0, sin(y)], [0, 1.0, 0],
[-sin(y), 0, cos(y)]])
R_EE = (RZ * RY * RX) * R_corr
R_EE = R_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
EE = Matrix([[px], [py], [pz]])
WC = EE - (0.303 * R_EE[:, 2])
# Calculate joint angles using Geometric IK method
#THETA1
theta1 = atan2(WC[1], WC[0])
#TRIANGLE FOR THETA 1 and 2
a = 1.501
b = sqrt(
pow((sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35), 2) +
pow((WC[2] - 0.75), 2))
c = 1.25
A = acos((b * b + c * c - a * a) / (2 * b * c))
B = acos((a * a + c * c - b * b) / (2 * a * c))
C = acos((b * b + a * a - c * c) / (2 * b * a))
#THETA2
theta2 = pi / 2 - A - atan2(
WC[2] - 0.75,
sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35)
#THETA3
theta3 = pi / 2 - (B + 0.036)
R0_3 = T0_1[0:3, 0:3] * T1_2[0:3, 0:3] * T2_3[0:3, 0:3]
R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
R3_6 = R0_3.inv("LU") * R_EE
#print("R3_6")
#print(R3_6[0],R3_6[1],R3_6[2])
#print(R3_6[3],R3_6[4],R3_6[5])
#print(R3_6[6],R3_6[7],R3_6[8])
#for i in range(len(R3_6)):
# if R3_6[i] < 10**(-4):
# R3_6[i] = 0
if 0.99 < R3_6[8] < 1.01:
theta6 = atan2(R3_6[0], R3_6[3])
theta4 = 0
theta5 = 0
print(theta4, theta5, theta6)
else:
theta5 = atan2(R3_6[8], sqrt(1 - R3_6[8]**2))
theta4 = atan2(R3_6[2], R3_6[4])
theta6 = atan2(-R3_6[6], R3_6[7])
theta5_2 = atan2(R3_6[8], -sqrt(1 - R3_6[8]**2))
theta4_2 = atan2(-R3_6[2], -R3_6[4])
theta6_2 = atan2(R3_6[6], -R3_6[7])
if theta5_2 < theta5:
theta5 = theta5_2
if theta6_2 < theta6:
theta6 = theta6_2
if theta4_2 < theta4:
theta4 = theta4_2
print('theta4', theta4, theta4_2, "theta5", theta5, theta5_2,
"theta6", theta6, theta6_2)
#THETA4
#theta4 = atan2(R3_6[2,2], -R3_6[0,2])
#THETA5
#theta5 = atan2(sqrt(R3_6[0,2]*R3_6[0,2] + R3_6[2,2]*R3_6[2,2]),R3_6[1,2])
#THETA6
#:theta6 = atan2(-R3_6[1,1], R3_6[1,0])
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
joint_trajectory_point.positions = [
theta1, theta2, theta3, theta4, theta5, theta6
]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
#print(joint_trajectory_list)
return CalculateIKResponse(joint_trajectory_list)
def joint_state_callback(msg):
global frame_xyz
global check_point_num
joint = []
for i in range(0, 5):
joint.append(msg.position[i])
# Forward kinematic to convert theta to 5 Transforamtion Martix
T = forward_kinematics(joint)
# Calculate each joint Oi_q from T01 to T05
Oi_q = {}
for i in range(5):
Oi_q[i] = [T[i][0, 3], T[i][1, 3], T[i][2, 3]]
# Set repulsive force parameter
rho = [0.1, 0.1, 1, 1, 1]
eta = [1, 1, 0.1, 1, 1]
# Calculate repulsive force
Force_repulsive = force_repulsive(Oi_q, Obstacle, eta, rho)
# print Force_repulsive
# Set attractive force parameter
zeta = [0.1, 0.1, 1, 1, 1]
d = [1, 1, 0.1, 1, 1]
# Calculate attractive force
Force_attractive = force_attractive(Oi_q, frame_xyz, zeta, d,
check_point_num)
# print 'Force_attractive', Force_attractive
jacobian = jacobian_o(joint)
#print jacobian
(torque, torque_mag) = compute_torque(Force_repulsive, Force_attractive,
jacobian)
# Gradient Descent parameter
alpha = (0.1, 0.1, 0.1, 0.1, 0.1)
tol = 0.05
# Calculate new position
torque = [
torque[0, 0], torque[0, 1], torque[0, 2], torque[0, 3], torque[0, 4]
]
[x * alpha / torque_mag for x in torque]
new_q = np.matrix(joint) + np.matrix(torque)
global angle
q_final = list(angle[check_point_num])
# Publish joint trajectory
msg = JointTrajectory()
msg.header.stamp = rospy.Time.now()
msg.joint_names = joint_names
point = JointTrajectoryPoint()
point.positions = [
new_q[0, 0], new_q[0, 1], new_q[0, 2], new_q[0, 3], new_q[0, 4]
]
msg.points.append(point)
traj_publisher.publish(msg)
print msg
qiqf_diff = (new_q[0, 0] - q_final[0], new_q[0, 1] - q_final[1],
new_q[0, 2] - q_final[2], new_q[0, 3] - q_final[3],
new_q[0, 4] - q_final[4])
qiqf_diff = LA.norm(qiqf_diff)
print qiqf_diff
if abs(qiqf_diff) < tol:
check_point_num = check_point_num + 1
def compute_IK(req, calculate_fk=False):
### Your FK code here
# Create symbols
q1, q2, q3, q4, q5, q6 = symbols('q1:7')
# Create Modified DH parameters
dh_transforms, constants = forward_dh_transform(q1, q2, q3, q4, q5, q6)
T0_1, T1_2, T2_3, T3_4, T4_5, T5_6, T6_g = dh_transforms
d1, a1, a2, a3, d4, dg = constants
R_corr = rot_z(pi) * rot_y(-pi / 2)
# Create individual transformation matrices
T0_4 = T0_1 * T1_2 * T2_3 * T3_4
T_corr = R_corr.row_join(Matrix([[0], [0], [0]])).col_join(Matrix([[0, 0, 0, 1]]))
T0_g = T0_4 * T4_5 * T5_6 * T6_g * T_corr
# Extract rotation matrices from the transformation matrices
R0_1 = T0_1[0:3, 0:3]
R1_2 = T1_2[0:3, 0:3]
R2_3 = T2_3[0:3, 0:3]
R0_3 = R0_1 * R1_2 * R2_3
###
# Initialize service response
joint_trajectory_list = []
for x in xrange(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(
[req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w])
### Your IK code here
# Rotation matrix in DH frame, compensated for rotation discrepancy between DH parameters and Gazebo
Rrpy = rot_z(yaw) * rot_y(pitch) * rot_x(roll) * R_corr.transpose()
# Target positions for end-effector and wrist center
pee_target = Matrix([[px], [py], [pz]])
pwc_target = pee_target - dg * Rrpy[:, 2]
# Calculate joint angles using Geometric IK method
# Position of wc gives first 3 q
theta1 = atan2(pwc_target[1], pwc_target[0])
h = pwc_target[2] - d1
l = sqrt(pwc_target[1] * pwc_target[1] + pwc_target[0] * pwc_target[0]) - a1
phi_interval = atan2(h, l)
A = sqrt(d4 * d4 + a3 * a3)
B = a2
C = sqrt(h * h + l * l)
a = acos((B * B + C * C - A * A) / (2 * B * C))
theta2 = pi / 2 - a - phi_interval
rho = atan2(a3, d4) + pi/2
c = acos((B * B + A * A - C * C) / (2 * B * A))
theta3 = pi - c - rho
# Orientation of end-effector gives last 3 q
R0_3_inv = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3}).transpose()
R3_6 = R0_3_inv * Rrpy
r13 = R3_6[0, 2]
r33 = R3_6[2, 2]
r23 = R3_6[1, 2]
r21 = R3_6[1, 0]
r22 = R3_6[1, 1]
p = sqrt(r13 * r13 + r33 * r33)
theta5 = atan2(p, r23)
theta4 = atan2(r33 / p, -r13 / p)
theta6 = atan2(-r22 / p, r21 / p)
###
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
joint_trajectory_point.positions = [theta1, theta2, theta3, theta4, theta5, theta6]
# Utility code for computing and returning forward kinematics results
pwc, pee, euler_angles = [None] * 3
if calculate_fk:
params = {q1: theta1,
q2: theta2,
q3: theta3,
q4: theta4,
q5: theta5,
q6: theta6}
T0_4_eval = T0_4.evalf(subs=params)
pwc = T0_4_eval[0:3, 3]
T0_g_eval = T0_g.evalf(subs=params)
pee = T0_g_eval[0:3, 3]
R_XYZ = T0_g_eval[0:3, 0:3]
r21 = R_XYZ[1, 0]
r11 = R_XYZ[0, 0]
r31 = R_XYZ[2, 0]
r32 = R_XYZ[2, 1]
r33 = R_XYZ[2, 2]
alpha = atan2(r21, r11) # rotation about Z-axis
beta = atan2(-r31, sqrt(r11 * r11 + r21 * r21)) # rotation about Y-axis
gamma = atan2(r32, r33) # rotation about X-axis
euler_angles = Matrix([gamma, beta, alpha])
cartesian_positions = (pwc, pee, euler_angles)
joint_trajectory_list.append((joint_trajectory_point, cartesian_positions))
return joint_trajectory_list
def test_joint_angles_duration_0(self): # https://github.com/start-jsk/rtmros_common/issues/1036
# for >= kinetic
larm = None
goal = None
if os.environ['ROS_DISTRO'] < 'kinetic' :
from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal
larm = actionlib.SimpleActionClient("/larm_controller/joint_trajectory_action", JointTrajectoryAction)
goal = JointTrajectoryGoal()
else:
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
larm = actionlib.SimpleActionClient("/larm_controller/follow_joint_trajectory_action", FollowJointTrajectoryAction)
goal = FollowJointTrajectoryGoal()
while self.joint_states == None:
time.sleep(1)
rospy.logwarn("wait for joint_states..")
## wait for joint_states updates
rospy.sleep(1)
current_positions = dict(zip(self.joint_states.name, self.joint_states.position))
# goal
goal.trajectory.header.stamp = rospy.get_rostime()
goal.trajectory.joint_names.append("LARM_SHOULDER_P")
goal.trajectory.joint_names.append("LARM_SHOULDER_R")
goal.trajectory.joint_names.append("LARM_SHOULDER_Y")
goal.trajectory.joint_names.append("LARM_ELBOW")
goal.trajectory.joint_names.append("LARM_WRIST_Y")
goal.trajectory.joint_names.append("LARM_WRIST_P")
goal.trajectory.points = []
## add current position
point = JointTrajectoryPoint()
point.positions = [ current_positions[x] for x in goal.trajectory.joint_names]
point.time_from_start = rospy.Duration(0.0)
goal.trajectory.points.append(point)
## add target position
point = JointTrajectoryPoint()
point.positions = [ x * math.pi / 180.0 for x in [15,15,15,-45,-20,-15] ]
point.time_from_start = rospy.Duration(1.0)
goal.trajectory.points.append(point)
## send goal
larm.send_goal(goal)
larm.wait_for_result()
## wait for joint_states updates
rospy.sleep(1)
current_positions = dict(zip(self.joint_states.name, self.joint_states.position))
goal.trajectory.points = []
## add current position
point = JointTrajectoryPoint()
point.positions = [ x * math.pi / 180.0 for x in [15,15,15,-45,-20,-15] ]
point.time_from_start = rospy.Duration(0.0)
goal.trajectory.points.append(point)
## add target position
point = JointTrajectoryPoint()
point.positions = [ x * math.pi / 180.0 for x in [30,30,30,-90,-40,-30] ]
point.time_from_start = rospy.Duration(1.0)
goal.trajectory.points.append(point)
## send goal again.... (# https://github.com/start-jsk/rtmros_common/issues/1036 requires send goal twice...)
larm.send_goal(goal)
larm.wait_for_result()
## wait for update joint_states
rospy.sleep(1)
current_positions = dict(zip(self.joint_states.name, self.joint_states.position))
goal_angles = [ 180.0 / math.pi * current_positions[x] for x in goal.trajectory.joint_names]
rospy.logwarn(goal_angles)
rospy.logwarn("difference between two angles %r %r"%(array([30,30,30,-90,-40,-30])-array(goal_angles),linalg.norm(array([30,30,30,-90,-40,-30])-array(goal_angles))))
self.assertAlmostEqual(linalg.norm(array([30,30,30,-90,-40,-30])-array(goal_angles)), 0, delta=0.1)
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
### Your FK code here
# Create symbols
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') #joint angles
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8') #link offset
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7') #link length
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols(
'alpha0:7') #twist angles
# Create Modified DH parameters
s = {
alpha0: 0,
a0: 0,
d1: 0.75,
q1: q1,
alpha1: -pi / 2.,
a1: 0.35,
d2: 0,
q2: q2 - pi / 2.,
alpha2: 0,
a2: 1.25,
d3: 0,
q3: q3,
alpha3: -pi / 2.,
a3: -0.054,
d4: 1.50,
q4: q4,
alpha4: pi / 2.,
a4: 0,
d5: 0,
q5: q5,
alpha5: -pi / 2.,
a5: 0,
d6: 0,
q6: q6,
alpha6: 0,
a6: 0,
d7: 0.303,
q7: 0
}
# Create individual transformation matrices
T0_1 = TF_Matrix(alpha0, a0, d1, q1).subs(s)
T1_2 = TF_Matrix(alpha1, a1, d2, q2).subs(s)
T2_3 = TF_Matrix(alpha2, a2, d3, q3).subs(s)
T3_4 = TF_Matrix(alpha3, a3, d4, q4).subs(s)
T4_5 = TF_Matrix(alpha4, a4, d5, q5).subs(s)
T5_6 = TF_Matrix(alpha5, a5, d6, q6).subs(s)
T6_EE = TF_Matrix(alpha6, a6, d7, q7).subs(s)
T0_EE = (T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_EE)
# ACcount for orientation difference between base link and end effector
Rot_y = Matrix([[cos(rad(-90)), 0, sin(rad(-90)), 0], [0, 1, 0, 0],
[-sin(rad(-90)), 0,
cos(rad(-90)), 0], [0, 0, 0, 1]])
Rot_z = Matrix([[cos(rad(180)), -sin(rad(180)), 0, 0],
[sin(rad(180)), cos(rad(180)), 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
R_corr = Rot_z * Rot_y
T_total = T0_EE * R_corr
# Extract rotation matrices from the transformation matrices
r, p, y = symbols('r p y')
rot_x = Matrix([[1, 0, 0], [0, cos(r), -sin(r)], [0, sin(r), cos(r)]])
rot_y = Matrix([[cos(p), 0, sin(p)], [0, 1, 0], [-sin(p), 0, cos(p)]])
rot_z = Matrix([[cos(y), -sin(y), 0], [sin(y), cos(y), 0], [0, 0, 1]])
rot_ee = rot_z * rot_y * rot_x
rot_err = rot_z.subs(y, rad(180)) * rot_y.subs(p, rad(-90))
rot_ee = rot_ee * rot_err
###
# Initialize service response
joint_trajectory_list = []
for x in xrange(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
### Your IK code here
rot_ee = rot_ee.subs({'r': roll, 'p': pitch, 'y': yaw})
# Calculate spehrical wrist center
ee_pos = Matrix([[px], [py], [pz]])
wc = ee_pos - (0.303) * rot_ee[:, 2]
# Calculate joint angles using Geometric IK method
# Theta1 can be found using the the wrist center position
theta1 = atan2(wc[1], wc[0])
#Theta2 and theta3 can be found using trigonometry. We have a triangle with 2 known sides:
# a = d4 = 1.501
# c = a2 = 1.25
# solve for b
a = 1.501
b = sqrt(
pow((sqrt(wc[0] * wc[0] + wc[1] * wc[1]) - 0.35), 2) +
pow((wc[2] - 0.75), 2))
c = 1.25
# Using law of cosines for an SSS triangle, we can find all the angles of the triangle
angle_a = acos((b * b + c * c - a * a) / (2 * b * c))
angle_b = acos((a * a + c * c - b * b) / (2 * a * c))
angle_c = acos((a * a + b * b - c * c) / (2 * a * b))
# Use angles and wrist center position to find theta2 and theta3
theta2 = pi / 2 - angle_a - atan2(
wc[2] - 0.75,
sqrt(wc[0] * wc[0] + wc[1] * wc[1]) - 0.35)
theta3 = pi / 2 - (angle_b + 0.036)
# Fill in rotation matrix from base link to link 3
R0_3 = T0_1[0:3, 0:3] * T1_2[0:3, 0:3] * T2_3[0:3, 0:3]
R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
R3_6 = R0_3.inv("LU") * rot_ee
# Euler angles from rotation matrix
theta4 = atan2(R3_6[2, 2], -R3_6[0, 2])
theta5 = atan2(
sqrt(R3_6[0, 2] * R3_6[0, 2] + R3_6[2, 2] * R3_6[2, 2]),
R3_6[1, 2])
theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
joint_trajectory_point.positions = [
theta1, theta2, theta3, theta4, theta5, theta6
]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
