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