b1 = [0,0,1,0,0,0]
b2 = [0,1,0,l1+l2+l3,0,0]
b3 = [0,0,1,0,0,0]
b4 = [0,1,0,l2+l3,0,w1]
b5 = [0,0,1,0,0,0]
b6 = [0,1,0,l3,0,0]
b7 = [0,0,1,0,0,0]
blist = np.array(([b1,b2,b3,b4,b5,b6,b7]))
theta = math.pi/2
theta_list = np.array(([theta],[theta],[theta],[theta],[theta],[theta],[theta]))
"""************************************
a) Numerical body jacobian
"""
jb = mr.JacobianBody(blist.T,theta_list)
print("a)")
print ("Jacobian: ")
pprint.pprint(jb)
print
print
"""************************************
b) principle axes and lengths
"""
"""split jacobian into jb_w and jb_v"""
jbw, jbv = np.rint(np.split(jb,2))
def IKinBodyIterates(Blist, M, T, thetalist0, eomg, ev,
filename='iterates.csv'):
"""
Computes inverse kinematics in the body frame for an open chain robot
outputting information at each iteration.
:param Blist: The joint screw axes in the end-effector frame when the
manipulator is at the home position, in the format of a
matrix with axes as the columns
:param M: The home configuration of the end-effector
:param T: The desired end-effector configuration Tsd
:param thetalist0: An initial guess of joint angles that are close to
satisfying Tsd
:param eomg: A small positive tolerance on the end-effector orientation
error. The returned joint angles must give an end-effector
orientation error less than eomg
:param ev: A small positive tolerance on the end-effector linear position
error. The returned joint angles must give an end-effector
position error less than ev
:param filename: The CSV filename to which joint value for each iteration
are output. Default is 'iterates.csv'.
:return thetalist: Joint angles that achieve T within the specified
tolerances,
:return success: A logical value where TRUE means that the function found
a solution and FALSE means that it ran through the set
number of maximum iterations without finding a solution
within the tolerances eomg and ev.
Uses an iterative Newton-Raphson root-finding method.
The maximum number of iterations before the algorithm is terminated has
been hardcoded in as a variable called maxiterations. It is set to 20 at
the start of the function, but can be changed if needed.
Example Input:
Blist = np.array([[0, 0, -1, 2, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 1, 0, 0, 0.1]]).T
M = np.array([[-1, 0, 0, 0],
[ 0, 1, 0, 6],
[ 0, 0, -1, 2],
[ 0, 0, 0, 1]])
T = np.array([[0, 1, 0, -5],
[1, 0, 0, 4],
[0, 0, -1, 1.6858],
[0, 0, 0, 1]])
thetalist0 = np.array([1.5, 2.5, 3])
eomg = 0.01
ev = 0.001
Output:
(np.array([1.57073819, 2.999667, 3.14153913]), True)
Writes information about each iteration to console and writes joint values
for each iteration to CSV file.
"""
'''
Prints iteration information to console.
'''
def printIterationSummary(i, thetalist, Tb, Vb, errAng, errLin):
print('Iteration %d' %i)
print('joint vector: %s' %thetalist)
print('end effector configuration:')
print('%s' %Tb)
print('twist error: %s' %Vb)
print('angular error magnitude: %s' %errAng)
print('linear error magnitude: %s' %errLin)
print('')
'''
Writes iteration joint values to CSV file.
'''
def writeJointValues(f, thetalist):
for i in range(len(thetalist)):
f.write('%s' %thetalist[i])
if i < len(thetalist) - 1:
f.write(',')
f.write('\n')
f = open(filename, 'w')
thetalist = np.array(thetalist0).copy()
i = 0
maxiterations = 20
err = True
Vb = np.zeros(6) # twist error
while err and i < maxiterations:
thetalist = thetalist + np.dot(np.linalg.pinv(mr.JacobianBody(Blist, thetalist)), Vb)
Tb = mr.FKinBody(M, Blist, thetalist) # current end effector configuration
Vb = mr.se3ToVec(mr.MatrixLog6(np.dot(mr.TransInv(Tb), T)))
errAng = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) # twist angular error
errLin = np.linalg.norm([Vb[3], Vb[4], Vb[5]]) # twist linear error
err = errAng > eomg or errLin > ev
printIterationSummary(i, thetalist, Tb, Vb, errAng, errLin)
writeJointValues(f, thetalist)
i += 1
f.close()
return (thetalist, not err)
Slist = np.array([s1, s2, s3])
JS = mr.JacobianSpace(Slist, thetalist)
print(JS)
print("\nQuestion 2 \n")
theta = np.array([[0.95], [0.98], [-0.92]])
b1 = np.array([1, 0, 0, 0, 0, 0]) # -4,0,0
b2 = np.array([1, 0, 0, 0, 0, 0]) #-6,0,0
b3 = np.array([0, 0, 0, 1, 0, 0]) # 0 -2 t
Blist = np.array([b1, b2, b3])
Blist = np.transpose(Blist)
JB = mr.JacobianBody(Blist, theta)
print(JB)
print("\n Question 3 \n")
T_1in0 = np.array([[0.85174056, 0.41765295, 0.31639222, 3.70016378],
[0.50645468, -0.81103411, -0.29279229, -7.42921783],
[0.13431932, 0.40962139, -0.90231294, 4.30158108],
[0.00000000, 0.00000000, 0.00000000, 1.00000000]])
M = np.array([[0.00000000, 1.00000000, 0.00000000, 6.00000000],
[1.00000000, 0.00000000, 0.00000000, -4.00000000],
[0.00000000, 0.00000000, -1.00000000, 0.00000000],
[0.00000000, 0.00000000, 0.00000000, 1.00000000]])
S = np.array([
[0.00000000, 0.00000000, 1.00000000, 0.00000000, 0.00000000, 0.00000000],
[-1.00000000, -1.00000000, 0.00000000, 0.00000000, 0.00000000, 1.00000000],
def calc_joint_vel(self):
""" Deprecated version of joint vel command calculation (Only feed-forward)
"""
rospy.logdebug("Calculating joint velocities...")
# Body stuff
Tbs = self.M0 #
Blist = self.Blist #
# Current joint angles
self.get_q_now()
with self.mutex:
q_now = self.q #1x7 mx
# Desired config: base to desired - Tbd
with self.mutex:
T_sd = self.T_goal #
self.current_time = time.time()
delta_time = self.current_time - self.last_time
# Find transform from current pose to desired pose, error
# refer to CH04 >> the product of exponential formula for open-chain manipulator
# sequence:
# 1. FKinBody (M, Blist, thetalist)
# M: the home config. of the e.e.
# Blist: the joint screw axes in the ee frame, when the manipulator is @ the home pose
# thetalist : a list of current joints list
# We get the new transformation matrix T for newly given joint angles
#
# 1). np.array(Blist)[:,i] >> retrieve one axis' joint screw
# 2). ex) [s10, -c10, 0., -1.0155*c10, -1.0155*s10, -0.1603] -> S(theta)
# 3). _out = VecTose3(_in)
# # Takes a 6-vector (representing a spatial velocity).
# # Returns the corresponding 4x4 se(3) matrix.
# 4). _out = MatrixExp6(_in)
# # Takes a se(3) representation of exponential coordinate
# # Returns a T matrix SE(3) that is achieved by traveling along/about the
# # screw axis S for a distance theta from an initial configuration T = I(dentitiy)
# 5). np.dot (M (from base to robot's e.e. @home pose , and multiplying exp coord(6), we get new FK pose
#
# 2. TransInv >> we get the inverse of homogen TF matrix
# 3. error = np.dot (T_new, T_sd) >> TF matrix from cur pose to desired pose
# 4. Vb >>compute the desired body twist vector from the TF matrix
# 5. JacobianBody:
# # In: Blist, and q_now >> Out : T IN SE(3) representing the end-effector frame when the joints are
# at the specified coordinates
e = np.dot(r.TransInv(r.FKinBody(Tbs, Blist, q_now)), T_sd) # Modern robotics pp 230 22Nff
# Desired TWIST: MatrixLog6 SE(3) -> se(3) exp coord
Vb = r.se3ToVec(r.MatrixLog6(e)) # shape : (6,)
# Construct BODY JACOBIAN for current config
Jb = r.JacobianBody(Blist, q_now) #6x7 mx # shape (6,7)
# WE NEED POSITION FEEDBACK CONTROLLER
# Desired ang vel - Eq 5 from Chiaverini & Siciliano, 1994
# Managing singularities: naive least-squares damping
n = Jb.shape[-1] #Size of last row, n = 7 joints
# OR WE CAN USE NUMPY' PINV METHOD
invterm = np.linalg.inv(np.dot(Jb.T, Jb) + pow(self.damping, 2)*np.eye(n)) #
qdot_new = np.dot(np.dot(invterm,Jb.T),Vb) # It seems little bit akward...? >>Eq 6.7 on pp 233 of MR book
self.qdot = qdot_new #1x7
# Constructing dictionary
qdot_output = dict(zip(self.names, self.qdot))
# Setting Sawyer right arm joint velocities
self.limb.set_joint_velocities(qdot_output)
# print qdot_output
return
def FeedbackControl(self, configuration, Xd, Xd_next, error_csv):
'''Input:
#configuration - phi, x, y, theta1, theta2, theta3, theta4, theta5
Xd - The end-effector reference configuration at the next timestep in the reference trajectory (Tse_des)
Xd_next - The end-effector reference configuration at a time delta_t later (Tse_des_next)
error_csv - path to the csv file
Output:
V - The commanded end-effector twist V expressed in the end-effector frame {e}.
velocities - commanded velocities (wheels 1-4, arm_joints 1-5)
Joint limits are implemented to avoid self-collisions and tolerance in Jacobian to avoid singularities
'''
thetalist = np.array(
configuration[3:8]) #find thetalist from a configuration vector
phi = configuration[0]
x = configuration[1]
y = configuration[2]
Tsb = KukaYoubot.calc_Tsb(
phi, x, y) # find Tsb (chassis base in the space frame)
T0e = mr.FKinBody(
KukaYoubot.M0e, KukaYoubot.Blist,
thetalist) # find T0e (end-effector in the arm base frame)
Tse_current = np.dot(
np.dot(Tsb, KukaYoubot.Tb0),
T0e) # current end-effector position in the space frame
Tse_current_desired = np.dot(
mr.TransInv(Tse_current),
Xd) # transformation from the current Tse to Tse_des
Tse_desired_next_desired = np.dot(
mr.TransInv(Xd), Xd_next
) # transformation from the desired position Tse_des to the next desired position Tse_des_next
Vd_matrix_form = mr.MatrixLog6(
Tse_desired_next_desired
) / self.timestep # The feedforward reference twist in adjoint matrix form
Vd = mr.se3ToVec(
Vd_matrix_form
) # The feedforward reference twist as 6 elements vector
Xerr_matrix_form = mr.MatrixLog6(
Tse_current_desired) # error twist in the 4x4 matrix form
Xerr = mr.se3ToVec(Xerr_matrix_form) #error twist as 6 elements vector
#here we save this error twist to the csv file
with open(error_csv, mode='a', newline='') as error_file:
error_writer = csv.writer(error_file,
delimiter=',',
quotechar='"',
quoting=csv.QUOTE_MINIMAL)
error_writer.writerow(Xerr)
feedforward = np.dot(
mr.Adjoint(Tse_current_desired), Vd
) # feedforward component calculated using formula 13.17 from the book
P = np.dot(
self.Kp, Xerr
) #proportional component calculated using formula 13.17 from the book
self.integral = self.integral + Xerr #calculation of the cumulative integral error
I = np.dot(
self.Ki, self.integral * self.timestep
) #integral component calculated using formula 13.17 from the book
V = feedforward + P + I # The commanded end-effector twist V expressed in the end-effector frame {e}.
Teb = np.dot(mr.TransInv(T0e), mr.TransInv(
KukaYoubot.Tb0)) # chassis base in the end-effector frame
Jbase = np.dot(
mr.Adjoint(Teb), KukaYoubot.F6
) #Jbase expresses the contribution of the wheel velocities u to the end-effector's velocity. Calculated using the formula from the page 552 in the book
Jarm = mr.JacobianBody(
KukaYoubot.Blist, thetalist
) #Body Jacobian of the arm expresses the contribution of the joint velocities to the end-effector's velocity
Je = np.hstack((Jbase, Jarm)) #resulting Jacobian
#Implementing joint limits to avoid self-collisions and singularities
while (True):
velocities = np.dot(
np.linalg.pinv(Je, 1e-3), V
) # The tolerance option allows you to specify how close to zero a singular value must be to be treated as zero. By treating small singular values (that are greater than the default tolerance) as zero, you will avoid having pseudoinverse matrices with unreasonably large entries
joints_with_constraints = self.FindJointLimits(
velocities[4:], thetalist)
if (len(joints_with_constraints)
): #if a list with constrained joints is contains elements:
for joint_constraint in joints_with_constraints:
Je[:, Jbase.shape[1] +
joint_constraint] = 0.0 #change corresponding column in the Jacobian to zeros
else:
break
return V, velocities
def FeedbackControl(self, config, Tse_des, Tse_des_next):
'''
Compute wheel and joint velocity required to achieve the desired configuration
Input:
config : {chasis: [phi, x , y], arm : [theta1, 2,3,4,5]}
Tse_des : Desired configuration of end effector wrt space frame
Tse_des_next : Desired configuration of end effector wrt space frame after timestep (used to calculate feedforward twist)
Return:
end_eff_twist : End effector twist wrt space frame required to move end effector from current transformation to the desired
vels[:4] : Corresponds to required wheel velocities
vels[4:] : Corresponds to required joint velocities
'''
thetalist = np.array(config["arm"])
phi = config["chasis"][0]
x = config["chasis"][1]
y = config["chasis"][2]
Tsb = np.array([[cos(phi),-sin(phi),0,x],[sin(phi),cos(phi),0,y],[0,0,1,self.height],[0,0,0,1]]) # chasis base wrt space frame
Toe = mr.FKinBody(self.Moe, self.Blist, thetalist) # base of the arm wrt to end effector
Tse_curr = np.dot(np.dot(Tsb,self.Tbo),Toe) # Tse = Tsb * Tbo * Toe
Tcurr_des = np.dot(mr.TransInv(Tse_curr),Tse_des) # Transform of desired wrt to current
Tdes_des_next = np.dot(mr.TransInv(Tse_des), Tse_des_next) # Transform of next_desired wrt desired
# Feedforward twist i.e twist required to transform Tdes to Tdes_next. This is for timestep hence 1/delta_time
feedforward_twist_se3 = mr.MatrixLog6(Tdes_des_next) / self.timestep
feedforward_twist_V = mr.se3ToVec(feedforward_twist_se3)
# Error twist i.e twist when done for unit time moves from Tcurr to Tdes
error_twist_se3 = mr.MatrixLog6(Tcurr_des)
error_twist_V = mr.se3ToVec(error_twist_se3)
# end_eff_twist (wrt space frame) = Adjoint(Tcurr_des)*Feedforward twist (Feedforward twist wrt to the current tranform) + Kp * error_twist + Ki * sum of error_twist
feedforward_term = np.dot(mr.Adjoint(Tcurr_des), feedforward_twist_V)
P_term = np.dot(self.Kp, error_twist_V)
self.integral = self.integral + error_twist_V
I_term = np.dot(self.Ki,self.integral*self.timestep)
end_eff_twist = feedforward_term + P_term + I_term # End effector twist wrt to space frame
Teb = np.dot(mr.TransInv(Toe), mr.TransInv(self.Tbo)) # chasis base wrt the end effector
# J_base is the component of base velocity contributing to end effector velocity
J_base = np.dot(mr.Adjoint(Teb),self.F6)
# J_b is the body jacobian
J_b = mr.JacobianBody(self.Blist, thetalist)
# J_e is the jacobian to convert end effector twist to joint speed and wheel velocity
J_e = np.hstack((J_base, J_b))
# Checking if joints exceeds the defined joint limits
while(True):
vels = np.dot(np.linalg.pinv(J_e,1e-3),end_eff_twist) # [u,theta_dot].. tolerance of 1e-3 is added to pinv to avoid close to singularity situation
constraint_joints = self.TestJointLimits(vels[4:], config["arm"])
if(len(constraint_joints)):
for const_joint in constraint_joints:
# Making the corresponding column of the constraint joints to zero in Je so that it indicates that these joints cannot be used to contribute to the end effector velocity
J_e[:,J_base.shape[1] + const_joint] = 0.0
else:
break
return end_eff_twist, vels[:4], vels[4:] # V, wheel speed, joint speed
def IKinBodyIterates(Blist, M, T, thetalist0, eomg, ev):
"""Computes inverse kinematics in the body frame for an open chain robot
:param Blist: The joint screw axes in the end-effector frame when the
manipulator is at the home position, in the format of a
matrix with axes as the columns
:param M: The home configuration of the end-effector
:param T: The desired end-effector configuration Tsd
:param thetalist0: An initial guess of joint angles that are close to
satisfying Tsd
:param eomg: A small positive tolerance on the end-effector orientation
error. The returned joint angles must give an end-effector
orientation error less than eomg
:param ev: A small positive tolerance on the end-effector linear position
error. The returned joint angles must give an end-effector
position error less than ev
:return thetalist: Joint angles that achieve T within the specified
tolerances,
:return success: A logical value where TRUE means that the function found
a solution and FALSE means that it ran through the set
number of maximum iterations without finding a solution
within the tolerances eomg and ev.
Uses an iterative Newton-Raphson root-finding method.
The maximum number of iterations before the algorithm is terminated has
been hardcoded in as a variable called maxiterations. It is set to 20 at
the start of the function, but can be changed if needed.
Example Input:
Blist = np.array([[0, 0, -1, 2, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 1, 0, 0, 0.1]]).T
M = np.array([[-1, 0, 0, 0],
[ 0, 1, 0, 6],
[ 0, 00, -1, 2],
[ 0, 0, 0, 1]])
T = np.array([[0, 1, 0, -5],
[1, 0, 0, 4],
[0, 0, -1, 1.6858],
[0, 0, 0, 1]])
thetalist0 = np.array([1.5, 2.5, 3])
eomg = 0.01
ev = 0.001
Output:
(np.array([1.57073819, 2.999667, 3.14153913]), True)
"""
thetalist = np.array(thetalist0).copy()
i = 0
csv = []
csv.append(list(thetalist0)) #To create a seperate matrix which will
maxiterations = 20 #save the values of joint vectors of each iteration
np.set_printoptions(precision=4)
Vb = mr.se3ToVec(
mr.MatrixLog6(np.dot(mr.TransInv(mr.FKinBody(M, Blist, thetalist)),
T)))
err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg or np.linalg.norm(
[Vb[3], Vb[4], Vb[5]]) > ev
while err and i < maxiterations:
print("Iteration {}:".format(i))
thetalist = thetalist + np.dot(
np.linalg.pinv(mr.JacobianBody(Blist, thetalist)), Vb)
print("Joint Vector:")
thetalist1 = [
'%.5f' % t for t in list(thetalist)
] #To print Vb without brackets and rounded off to 5 digits
print(*thetalist1, sep=', ')
print()
i = i + 1
Vb = mr.se3ToVec(
mr.MatrixLog6(
np.dot(mr.TransInv(mr.FKinBody(M, Blist, thetalist)), T)))
print("SE(3) end−effector config:")
print(
re.sub(
'[\[\]]', '',
np.array_str(
mr.MatrixLog6(
np.dot(mr.TransInv(mr.FKinBody(M, Blist, thetalist)),
T))))
) #To print the SE(3) end−effector configuration
print() #without brackets
print("error twist V_b:")
Vb1 = ['%.5f' % b for b in list(Vb)]
print(*Vb1, sep=', '
) #To print Vb without brackets and rounded off to 5 digits
print()
err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg or np.linalg.norm(
[Vb[3], Vb[4], Vb[5]]) > ev
print("angular error magnitude ||omega_b||: ",
np.linalg.norm([Vb[0], Vb[1],
Vb[2]])) #To print the angular error magnitude
print()
csv.append(list(thetalist))
print("linear error magnitude ||v_b||: ",
np.linalg.norm([Vb[3], Vb[4],
Vb[5]])) #To print the linear error magnitude
print()
np.savetxt("iterates.csv", csv,
delimiter=',') #To save csv array as a .csv file
return (thetalist, not err)
import modern_robotics as mr import numpy as np import math Slist = np.array([[0,0,1,0,0,0],[1,0,0,0,2,0],[0,0,0,0,1,0]]).T thetaList = np.array([math.pi/2,math.pi/2,1]) Blist = np.array([[0,1,0,3,0,0],[-1,0,0,0,3,0],[0,0,0,0,0,1]]).T Js = mr.JacobianSpace(Slist, thetaList) Jb = mr.JacobianBody(Blist, thetaList) # Slist = np.array([[0, 0, 1, 0, 0.2, 0.2], # [1, 0, 0, 2, 0, 3], # [0, 1, 0, 0, 2, 1], # [1, 0, 0, 0.2, 0.3, 0.4]]).T # thetalist = np.array([0.2, 1.1, 0.1, 1.2]) Jvel = np.array([[-0.105,0,0.006,-0.045,0,0.006,0],[-0.889,0.006,0,-0.844,0.006,0,0],[0,-0.105,0.889,0,0,0,0]]) A = np.dot(Jvel, Jvel.T) eigenA = np.linalg.eig(A) print eigenA
def calc_joint_vel(self):
rospy.logdebug("Calculating joint velocities...")
# Body stuff
Tbs = self.M0
Blist = self.Blist
# Desired config: base to desired - Tbd
with self.mutex:
q_now = self.q
T_cur = r.FKinBody(Tbs, Blist, q_now)
T_sd = np.empty_like(self.T_goal)
if self.is_ref_pose_cb == True:
T_sd = self.T_goal
else :
np.copyto(T_sd, T_cur)
e = np.zeros(6)
e[0:3] = self.get_phi(T_sd[0:3,0:3],T_cur[0:3,0:3])
e[3:6] = T_sd[0:3,3] - T_cur[0:3,3]
# Construct BODY JACOBIAN for current config
Jb = r.JacobianBody(Blist, q_now) #6x7 mx
Jv = Jb[3:6,:]
x_dot_pos = np.dot(Jv, q_now)
# Desired ang vel - Eq 5 from Chiaverini & Siciliano, 1994
# Managing singularities: naive least-squares damping
invterm = np.linalg.inv(np.dot(Jv,Jv.T)+ pow(self.damping, 2)*np.eye(3))
kp = 6.0
kv = 0.0
qdot_new = np.dot(np.dot(Jv.T, invterm), kp*e[3:6] - kv*x_dot_pos)
#Scaling joint velocity
minus_v = abs(np.amin(qdot_new))
plus_v = abs(np.amax(qdot_new))
if minus_v > plus_v:
scale = minus_v
else:
scale = plus_v
if scale > self.joint_vel_limit:
qdot_new = 2.0*(qdot_new/scale)*self.joint_vel_limit
self.qdot = qdot_new #1x7
dt = 0.01
q_desired = q_now + qdot_new * dt
self.j1_pos_command_pub.publish(q_desired[0])
self.j2_pos_command_pub.publish(q_desired[1])
self.j3_pos_command_pub.publish(q_desired[2])
self.j4_pos_command_pub.publish(q_desired[3])
self.joint_pos_command_to_dxl_pub.publish(data=q_desired)
ee_pose = Pose()
ee_pose.position.x = T_cur[0,3]
ee_pose.position.y = T_cur[1,3]
ee_pose.position.z = T_cur[2,3]
self.end_effector_pose_pub.publish(ee_pose)
return
[r31, r32, r33, pz], [0, 0, 0, 1]])
thetaBlist = np.array(([J1, J2, J3, J4, J5]))
Tsb = np.array([[cos(chassis_phi), -sin(chassis_phi), 0, chassis_x],
[sin(chassis_phi),
cos(chassis_phi), 0, chassis_y], [0, 0, 1, 0.0963],
[0, 0, 0, 1]])
Tse_home = np.matmul(Tsb, Tbe)
Xse = mr.FKinBody(Tse_home, Blist, thetaBlist)
X0e = mr.FKinBody(M0e, Blist, thetaBlist)
Jarm = mr.JacobianBody(Blist, thetaBlist)
Jbase = mr.Adjoint(np.linalg.inv(X0e) @ np.linalg.inv(Tb0)) @ F6
Je = np.hstack((Jbase, Jarm))
iJe = pinv(Je, 3e-3)
Ki = 0.001 * np.eye(6)
Kp = 2 * np.eye(6)
Xerr = mr.se3ToVec(mr.MatrixLog6((np.matmul(np.linalg.inv(Xse), Xref))))
acc_error = acc_error + Xerr * dt
V = FeedbackControl(Xse, Xref, Xref_next, Kp, Ki, dt, Xerr, acc_error)
Xerr_lst = Xerr
csv_output = "%10.6f, %10.6f, %10.6f, %10.6f, %10.6f, %10.6f, %10.6f \n" % (
t_int, Xerr_lst[0], Xerr_lst[1], Xerr_lst[2], Xerr_lst[3], Xerr_lst[4],
def IKinBodyIterates(Blist, M, T, thetalist0, eomg, ev):
"""Computes inverse kinematics in the body frame for an open chain robot
:param Blist: The joint screw axes in the end-effector frame when the
manipulator is at the home position, in the format of a
matrix with axes as the columns
:param M: The home configuration of the end-effector
:param T: The desired end-effector configuration Tsd
:param thetalist0: An initial guess of joint angles that are close to
satisfying Tsd
:param eomg: A small positive tolerance on the end-effector orientation
error. The returned joint angles must give an end-effector
orientation error less than eomg
:param ev: A small positive tolerance on the end-effector linear position
error. The returned joint angles must give an end-effector
position error less than ev
:return thetalist: Joint angles that achieve T within the specified
tolerances,
:return success: A logical value where TRUE means that the function found
a solution and FALSE means that it ran through the set
number of maximum iterations without finding a solution
within the tolerances eomg and ev.
Uses an iterative Newton-Raphson root-finding method.
The maximum number of iterations before the algorithm is terminated has
been hardcoded in as a variable called maxiterations. It is set to 20 at
the start of the function, but can be changed if needed.
Example Input:
Blist = np.array([[0, 0, -1, 2, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 1, 0, 0, 0.1]]).T
M = np.array([[-1, 0, 0, 0],
[ 0, 1, 0, 6],
[ 0, 0, -1, 2],
[ 0, 0, 0, 1]])
T = np.array([[0, 1, 0, -5],
[1, 0, 0, 4],
[0, 0, -1, 1.6858],
[0, 0, 0, 1]])
thetalist0 = np.array([1.5, 2.5, 3])
eomg = 0.01
ev = 0.001
Output:
(np.array([1.57073819, 2.999667, 3.14153913]), True)
"""
thetalist = np.array(thetalist0).copy()
print "Starting Inverse Kinematic Calculation in Body Frame, with initial joint vector: ", thetalist
i = 0
maxiterations = 20
err = True
JointVectorMatrix = []
while i < maxiterations:
print "========== Iteration ", i, " =========="
print "Joint Vector: ", thetalist
JointVectorMatrix.append(thetalist.tolist())
# Calculating forward kinematics (Current Configuration) : Tsb
Tsb = mr.FKinBody(M, Blist, thetalist)
print "SE(3) end-effector config: "
with np.printoptions(precision=3, suppress=True):
print(Tsb)
# Calculating error Twist : V
Vb = mr.se3ToVec(mr.MatrixLog6(np.dot(mr.TransInv(Tsb), T)))
print "Error twist V_b: "
with np.printoptions(precision=3, suppress=True):
print(Vb)
#Calculating error
angularVelError = np.linalg.norm([Vb[0], Vb[1], Vb[2]])
linearVelError = np.linalg.norm([Vb[3], Vb[4], Vb[5]])
err = angularVelError > eomg or linearVelError > ev
print "Angular error magnitude ||omega_b||: ", angularVelError
print "Linear error magnitude ||v_b||: ", linearVelError
if err:
# Error is not within acceptable limits. Calculate new joint angles
thetalist = thetalist + np.dot(
np.linalg.pinv(mr.JacobianBody(Blist, thetalist)), Vb)
i = i + 1
else:
# Error is within acceptable limits. End iterations
break
# Save joint vectors to CSV file
np.savetxt("iterates.csv", np.array(JointVectorMatrix), delimiter=",")
return (thetalist, not err)
def FeedbackControl(config, Xd, Xd_next, Kp, Ki, dt, jointlimit):
# here we compute X
M = np.array([[1, 0, 0, 0.033], [0, 1, 0, 0], [0, 0, 1, 0.6546],
[0, 0, 0, 1]])
Blist = np.array([[0, 0, 1, 0, 0.033, 0], [0, -1, 0, -0.5076, 0, 0],
[0, -1, 0, -0.3526, 0, 0], [0, -1, 0, -0.2176, 0, 0],
[0, 0, 1, 0, 0, 0]]).T
Tb0 = np.array([[1, 0, 0, 0.1662], [0, 1, 0, 0], [0, 0, 1, 0.0026],
[0, 0, 0, 1]])
thetalist_initial = np.array(
[config[0, 3], config[0, 4], config[0, 5], config[0, 6], config[0, 7]])
Tsb_initial = np.array(
[[np.cos(config[0, 0]), -np.sin(config[0, 0]), 0, config[0, 1]],
[np.sin(config[0, 0]),
np.cos(config[0, 0]), 0, config[0, 2]], [0, 0, 1, 0.0963],
[0, 0, 0, 1]])
X = Tsb_initial.dot(Tb0).dot(mr.FKinBody(M, Blist, thetalist_initial))
# here we write down Vd
Vd = mr.se3ToVec((1 / dt) * mr.MatrixLog6(np.linalg.inv(Xd).dot(Xd_next)))
# here we write down Vb = Ad*Vd
Vb = mr.Adjoint(np.linalg.inv(X).dot(Xd)).dot(Vd)
# here we write down X_err
X_err = mr.se3ToVec(mr.MatrixLog6(np.linalg.inv(X).dot(Xd)))
# here we write down commanded twist
V = Vb + Kp * X_err + Ki * (X_err + X_err * dt)
# here we compute Je
# first we write down J_arm
Blist = np.array([[0, 0, 1, 0, 0.033, 0], [0, -1, 0, -0.5076, 0, 0],
[0, -1, 0, -0.3526, 0, 0], [0, -1, 0, -0.2176, 0, 0],
[0, 0, 1, 0, 0, 0]]).T
thetalist = np.array(
[config[0, 3], config[0, 4], config[0, 5], config[0, 6], config[0, 7]])
J_arm = mr.JacobianBody(Blist, thetalist)
# second we write down J_base
r = 0.0475
l = 0.47 / 2
w = 0.3 / 2
gamma1 = -np.pi / 4
gamma2 = np.pi / 4
gamma3 = -np.pi / 4
gamma4 = np.pi / 4
beta = 0
x1 = l
y1 = w
x2 = l
y2 = -w
x3 = -l
y3 = -w
x4 = -l
y4 = w
# here we write down F6
a1 = np.array([1, np.tan(gamma1)])
a2 = np.array([1, np.tan(gamma2)])
a3 = np.array([1, np.tan(gamma3)])
a4 = np.array([1, np.tan(gamma4)])
b = np.array([[np.cos(beta), np.sin(beta)], [-np.sin(beta), np.cos(beta)]])
c1 = np.array([[-y1, 1, 0], [x1, 0, 1]])
c2 = np.array([[-y2, 1, 0], [x2, 0, 1]])
c3 = np.array([[-y3, 1, 0], [x3, 0, 1]])
c4 = np.array([[-y4, 1, 0], [x4, 0, 1]])
h1 = (((1 / r) * a1).dot(b)).dot(c1)
h2 = (((1 / r) * a2).dot(b)).dot(c2)
h3 = (((1 / r) * a3).dot(b)).dot(c3)
h4 = (((1 / r) * a4).dot(b)).dot(c4)
H0 = np.vstack((h1, h2, h3, h4))
F = np.linalg.pinv(H0)
F6 = np.array([[0, 0, 0, 0], [0, 0, 0, 0],
[F[0, 0], F[0, 1], F[0, 2], F[0, 3]],
[F[1, 0], F[1, 1], F[1, 2], F[1, 3]],
[F[2, 0], F[2, 1], F[2, 2], F[2, 3]], [0, 0, 0, 0]])
# then write down J_base
Tsb = np.array(
[[np.cos(config[0, 0]), -np.sin(config[0, 0]), 0, config[0, 1]],
[np.sin(config[0, 0]),
np.cos(config[0, 0]), 0, config[0, 2]], [0, 0, 1, 0.0963],
[0, 0, 0, 1]])
Teb = np.linalg.inv(X).dot(Tsb)
J_base = mr.Adjoint(Teb).dot(F6)
# here we test joint limits
if jointlimit == 1:
testJointLimits(config, J_arm)
# finally we write down Je
Je = np.hstack((J_base, J_arm))
# here we write down speeds (u,thetadot)
speeds = np.linalg.pinv(Je, rcond=1e-2).dot(V)
return V, speeds, X_err
