def getnextState(currentState, controls): """ takes the current state of the robot and calculates the next state. based on odomentery for chasis and First order euler intergration for joint of the manipulator input: current state: instance of the youBot state class controlVector : instance of the youBot control vector class returns: the state of the robot at time t. """ controls.jointSpeeds = youBotProperties.saturate( controls.jointSpeeds, youBotProperties.jointSpeedLimit) controls.wheelSpeeds = youBotProperties.saturate( controls.wheelSpeeds, youBotProperties.wheelSpeedLimit) currentState.jointState = currentState.jointState + youBotProperties.deltaT * controls.jointSpeeds temp = currentState.wheelState + youBotProperties.deltaT * controls.wheelSpeeds deltaTheta = temp - currentState.wheelState currentState.wheelState = temp Vb6 = np.dot(youBotProperties.F6, deltaTheta) Tbb_ = mr.MatrixExp6(mr.VecTose3(Vb6)) Tsbnew = np.dot(basePosition(currentState.chasisState), Tbb_) phi, x, y = atan2(Tsbnew[1, 0], Tsbnew[0, 0]), Tsbnew[0, 3], Tsbnew[1, 3] currentState.chasisState = np.array([phi, x, y])
def NextState(chassis_phi, chassis_x, chassis_y, J1, J2, J3, J4, J5, W1, W2, W3, W4 \ ,gripper_state, J_speed1, J_speed2, J_speed3, J_speed4, J_speed5, u1,u2,u3,u4, dt, c): r = 0.0475 w = 0.3/2 l = 0.47/2 new_J1 = J1 + J_speed1*dt new_J2 = J2 + J_speed2*dt new_J3 = J3 + J_speed3*dt new_J4 = J4 + J_speed4*dt new_J5 = J5 + J_speed5*dt new_W1 = W1 + u1*dt new_W2 = W2 + u2*dt new_W3 = W3 + u3*dt new_W4 = W4 + u4*dt u = np.mat([[u1],[u2],[u3],[u4]]) F = r/4*np.array([[-1/(l+w), 1/(l+w), 1/(l+w), -1/(l+w)], [1.0, 1.0, 1.0, 1.0], [-1.0, 1.0, -1.0, 1.0]]) Vb = np.matmul(F,u) #Vb6 = [0.0,0.0,np.asscalar(Vb[0]),np.asscalar(Vb[1]),np.asscalar(Vb[2]),0.0] #print(Vb6) Vb6 = np.array([0, 0, np.asscalar(Vb[0]), np.asscalar(Vb[1]), np.asscalar(Vb[2]), 0]) _Vb6_ = mr.VecTose3(Vb6) # _Vb6_ = np.array([[0, np.asscalar(-Vb[0]), 0, np.asscalar(Vb[1])], # [np.asscalar(Vb[0]), 0, 0, np.asscalar(Vb[2])], # [0, 0, 0, 0], # [0, 0, 0, 0]]) #print("[Vb6] =",_Vb6_) # _Vb_ = [[0.0, np.asscalar(-Vb[2]), np.asscalar(Vb[1])], # [np.asscalar(Vb[2]), 0.0, np.asscalar(Vb[0])], # [np.asscalar(Vb[1]), np.asscalar(Vb[0]), 0.0]] # # # print((_Vb_)) # # _Vb6_ = np.array([np.zeros(3), np.zeros(3), _Vb_[0], _Vb_[1], _Vb_[2], np.zeros(3)]) # print("\n",_Vb6_) 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]]) #print("\nTsb =",Tsb) Tb1 = mr.MatrixExp6(_Vb6_*dt) #print("\nTb1 =", Tb1) Tsb1 = np.matmul(Tsb,Tb1) #print("\nTsb1 =",Tsb1,Tsb1[0][0]) new_chassis_phi = acos(Tsb1[0][0]) new_chassis_x = Tsb1[0][3] new_chassis_y = Tsb1[1][3] # print("\nchassis_x = ",new_chassis_x) # print("\nchassis_y = ",new_chassis_y) #print("\nchassis_phi = ",chassis_phi) return new_chassis_phi, new_chassis_x, new_chassis_y, new_J1, new_J2, new_J3, new_J4, new_J5, new_W1, new_W2, new_W3, new_W4
def computeConfig(S, angle): try: Sn = S * angle T0n = mr.VecTose3(Sn) Tn = np.matmul(mr.MatrixExp6(T0n),T) return Tn except: print("Failed to calculate!")
def forward_kinematics(joints): # input: joint angles [joint1, joint2, joint3, joint4] # output: the position of end effector [x, y, z] L = 1 joint1 = joints[0] joint2 = joints[1] joint3 = joints[2] joint4 = joints[3] if joint3 > 1.2 or joint3 < 0: raise ValueError( "Invalid value for joint3, must be between 0 and 2 meters") #precalculated M, [S3], [S2] and [S1] M = np.array([[1, 0, 0, 0], [0, 1, 0, 3 * L], [0, 0, 1, 3 * L], [0, 0, 0, 1]]) S4_bracket = np.array([[0, 0, 0, 0], [0, 0, -1, 3 * L], [0, 1, 0, -2 * L], [0, 0, 0, 0]]) S3_bracket = np.array([[0, 0, 0, 0], [0, 0, 0, 1], [0, 0, 0, 0], [0, 0, 0, 0]]) S2_bracket = np.array([[0, 0, 0, 0], [0, 0, -1, 3 * L], [0, 1, 0, 0], [0, 0, 0, 0]]) S1_bracket = np.array([[0, -1, 0, 0], [1, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0]]) #exponential term S4_theta = joint4 * S4_bracket S3_theta = joint3 * S3_bracket S2_theta = joint2 * S2_bracket S1_theta = joint1 * S1_bracket #matrix exponential S4_exp = mr.MatrixExp6(S4_theta) S3_exp = mr.MatrixExp6(S3_theta) S2_exp = mr.MatrixExp6(S2_theta) S1_exp = mr.MatrixExp6(S1_theta) #left-multiplying T04 = np.dot(S4_exp, M) T03 = np.dot(S3_exp, T04) T02 = np.dot(S2_exp, T03) T01 = np.dot(S1_exp, T02) R, p = mr.TransToRp(T01) x = p[0] y = p[1] z = p[2] print[x, y, z] return [x, y, z]
def p14(): print('P14') se3mat = np.array([[0, -1.5708, 0, 2.3562], [1.5708, 0, 0, -2.3562], [0, 0, 0, 1], [0, 0, 0, 0]]) result = mr.MatrixExp6(se3mat) print(result)
def nextState(current_config, control_config): """ current_config: phi, x, y, Chassis j1, j2, j3, j4, j5 Arm w1, w2, w3, w4 Wheels g Gripper control_config: j1d, j2d, j3d, j4d, j5d Arm joint Velocites dt: Timestamp delta_t MAX_SPEED: Maximum angular speed of the arm joints and the wheels (rad/s) """ # Unpack current_config phi, x, y, j1, j2, j3, j4, j5, w1, w2, w3, w4, g = current_config # Unpack control_config j1d, j2d, j3d, j4d, j5d, w1d, w2d, w3d, w4d = control_config # Define Tsb Tsb = np.array([[np.cos(phi), -np.sin(phi), 0, x], [np.sin(phi), np.cos(phi), 0, y], [0, 0, 1, 0.0963], [0, 0, 0, 1]]) # wz, vx, vy wdt = np.array([[w1d], [w2d], [w3d], [w4d]]) * dt Vb = (r / 4) * np.dot(H_pseudo, wdt) Vb6 = np.transpose([[0, 0, Vb[0], Vb[1], Vb[2], 0]]) Tbb = mr.MatrixExp6(mr.VecTose3(Vb6)) Tb = np.dot(Tsb, Tbb) pp = np.arccos(Tb[0][0]) xx = Tb[0][3] yy = Tb[1][3] j1_new = j1 + (j1d * dt) j2_new = j2 + (j2d * dt) j3_new = j3 + (j3d * dt) j4_new = j4 + (j4d * dt) j5_new = j5 + (j5d * dt) w1_new = w1 + (w1d * dt) w2_new = w2 + (w2d * dt) w3_new = w3 + (w3d * dt) w4_new = w4 + (w4d * dt) result.append([ pp, xx, yy, j1_new, j2_new, j3_new, j4_new, j5_new, w1_new, w2_new, w3_new, w4_new, g ]) return pp, xx, yy, j1_new, j2_new, j3_new, j4_new, j5_new, w1_new, w2_new, w3_new, w4_new, g
def NextState(curConfig,controls,del_t,limits): nextState = [] curJointConfig = np.array(curConfig[3:8]) q_cur= np.array(curConfig[0:3]) curWheelConfig = np.array(curConfig[8:]) jointSpeeds = np.array(controls[0:5]) u = np.array(controls[5:]) ## checking limits for i in range(len(controls)): if controls[i] > limits: controls[i] = limits elif controls[i] < -limits: controls[i] = -limits ## Euler step for joints and wheels nextJointConfig = curJointConfig + jointSpeeds*del_t nextWheelConfig = curWheelConfig + u*del_t ## YouBot Properties: l = 0.47/2 w = 0.3/2 r = 0.0475 phi_k = q_cur[0] ## Odometry calculations for chassis Euler step F = (r/4)*np.array([[-1/(l+w), 1/(l+w), 1/(l+w),-1/(l+w)],[1, 1, 1, 1],[-1, 1, -1, 1]]) del_theta = u*del_t Vb = np.dot(F,del_theta) Vb6 = np.array([0,0,Vb[0],Vb[1],Vb[2],0]) [R,p] = mr.TransToRp(mr.MatrixExp6(mr.VecTose3(Vb6))) omg = mr.so3ToVec(mr.MatrixLog3(R)) del_x = p[0] del_y = p[1] del_w = omg[2] if del_w == 0: del_qb = np.array([del_w,del_x,del_y]) else: del_qb = np.array([del_w, \ (del_x*m.sin(del_w) + del_y*(m.cos(del_w)-1))/del_w, \ (del_y*m.sin(del_w) + del_y*(1-m.cos(del_w)))/del_w]) del_q = np.dot(np.array([[1,0,0],[0,m.cos(phi_k),-m.sin(phi_k)],[0,m.sin(phi_k),m.cos(phi_k)]]), \ del_qb) q_next = q_cur + del_q nextConfig = [list(q_next),list(nextJointConfig),list(nextWheelConfig)] nextState = list(itertools.chain(*nextConfig)) return nextState
def NextState(currentconfiguration, controls, dt, maximum): matrix = np.zeros(12) for i in range(5, 9): if controls[i] < -maximum: controls[i] = -maximum # Setting Wheel Speed Limit elif controls[i] > maximum: controls[i] = maximum matrix[3:12] = currentconfiguration[3:12] + ( dt * controls[0:9] ) # Calculating next step joint angle and wheel speed A = np.transpose(matrix[8:12] - currentconfiguration[8:12]) B = np.dot(HI, A) v = np.zeros(6) v[2:5] = B se3mat = mr.VecTose3(v) # Twist T = mr.MatrixExp6(se3mat) # Tbb+1 Matrix matrix[0] = currentconfiguration[0] + np.arccos(T[0, 0]) matrix[1] = currentconfiguration[1] + T[0, 3] matrix[2] = currentconfiguration[2] + T[1, 3] return matrix
def NextState(config, control, dt, speed, g): phi, x, y, a1, a2, a3, a4, a5, w1, w2, w3, w4, _ = config[0] a1dot, a2dot, a3dot, a4dot, a5dot, w1dot, w2dot, w3dot, w4dot = control[0] tsb = np.array([[cos(phi), -sin(phi), 0, x], [sin(phi), cos(phi), 0, y], [0, 0, 1, 0.0963], [0, 0, 0, 1]]) theta_del = np.array([[w1dot, w2dot, w3dot, w4dot]])*dt Vb = np.dot(h_zero, theta_del.T)*r/4 Vb6 = np.array([[0, 0, Vb[0][0], Vb[1][0], Vb[2][0], 0]]).T Vb6_vec = mr.VecTose3(Vb6) Tb_bk = mr.MatrixExp6(Vb6_vec) Tsb = np.dot(tsb, Tb_bk) #new orientation of robot phi_ = np.arcsin(Tsb[1][0]) x_ = Tsb[0][3] y_ = Tsb[1][3] #new arm joint angle a1 = a1+a1dot*dt a2 = a2+a2dot*dt a3 = a3+a3dot*dt a4 = a4+a4dot*dt a5 = a5+a5dot*dt #new arm joint angle w1 = w1+w1dot*dt w2 = w2+w2dot*dt w3 = w3+w3dot*dt w4 = w4+w4dot*dt #used for checking if phi is an instance of numpy arrays if(isinstance(phi_, np.ndarray)): new_config = [phi_[0], x_[0], y_[0], a1, a2, a3, a4, a5, w1, w2, w3, w4, g] for_next_state.append([phi_[0], x_[0], y_[0], a1, a2, a3, a4, a5, w1, w2, w3, w4, g]) else: new_config = [phi_, x_, y_, a1, a2, a3, a4, a5, w1, w2, w3, w4, g] for_next_state.append([phi_, x_, y_, a1, a2, a3, a4, a5, w1, w2, w3, w4, g]) return new_config #next state return value
def NextState(currentConfig,control,dt,maxSpeed,gripper): """ currentConfig: Chassis phi, Chassis x, Chassis y, J1, J2, J3, J4, J5, W1, W2, W3, W4, Gripper control: Arm joint speed(thetadot), Wheel speed(wdot) dt: timestamp maxSpeed: Maximum angular speed of the arm joints and the wheels """ phi,x,y,j1,j2,j3,j4,j5,w1,w2,w3,w4,crap = currentConfig[0] j1dot,j2dot,j3dot,j4dot,j5dot,w1dot,w2dot,w3dot,w4dot = control[0] Ts = np.array([[cos(phi), -sin(phi), 0, x],[sin(phi), cos(phi), 0, y],[0, 0, 0, 0.0963],[0, 0, 0, 1]]) dtheta = np.array([[w1dot],[w2dot],[w3dot],[w4dot]])*dt Vb = np.dot(Hpsu,dtheta)*r/4 Vb6 = np.array([[0,0,Vb[0],Vb[1],Vb[2],0]]).T Tbb = mr.MatrixExp6(mr.VecTose3(Vb6)) Tb = np.dot(Ts,Tbb) phiphi = np.arccos(Tb[0][0]) xx = Tb[0][3] yy = Tb[1][3] """ Update new joint angles and wheel speeds """ jj1 = j1 + j1dot * dt jj2 = j2 + j2dot * dt jj3 = j3 + j3dot * dt jj4 = j4 + j4dot * dt jj5 = j5 + j5dot * dt ww1 = w1 + w1dot * dt ww2 = w2 + w2dot * dt ww3 = w3 + w3dot * dt ww4 = w4 + w4dot * dt result.append([phiphi,xx,yy,jj1,jj2,jj3,jj4,jj5,ww1,ww2,ww3,ww4,gripper]) return phiphi,xx,yy,jj1,jj2,jj3,jj4,jj5,ww1,ww2,ww3,ww4,gripper
def p2p_cart_cubic_screw(robot, T_end, ts): T_start = robot.get_homogeneous() a2 = 3 / (ts**2) a3 = -2 / (ts**3) t = 0 while t < ts: t += robot.dt s = a2 * t**2 + a3 * t**3 T = T_start @ mr.MatrixExp6( mr.MatrixLog6(np.linalg.inv(T_start) @ T_end) * s) pos = T[0:3, 3] mat_rot = T[0:3, 0:3] eul = rot2eul(mat_rot) quat = p.getQuaternionFromEuler(eul) joints = p.calculateInverseKinematics( robot.robot_id, endEffectorLinkIndex=robot.eef_link_idx, targetPosition=pos, targetOrientation=quat) yield joints
import modern_robotics as mr import numpy as np T = np.array([[0, -1, 0, 3], [1, 0, 0, 0], [0, 0, 1, 1], [0, 0, 0, 1]]) x = mr.TransInv(T) print(x) T = np.array([[1], [0], [0], [0], [2], [3]]) x = mr.VecTose3(T) print(x) q = np.array([0, 0, 2]) s = np.array([1, 0, 0]) h = 1 x = mr.ScrewToAxis(q, s, h) print(x) T = ([[0, -1.5708, 0, 2.3562], [1.5708, 0, 0, -2.3562], [0, 0, 0, 1], [0, 0, 0, 0]]) x = mr.MatrixExp6(T) print(x) T = ([[0, -1, 0, 3], [1, 0, 0, 0], [0, 0, 1, 1], [0, 0, 0, 1]]) x = mr.MatrixLog6(T) print(x)
# Determinant is 1 assert math.isclose(np.linalg.det(R), 1) # R^-1 = R^T inv = np.linalg.inv(R) transp = np.transpose(R) assert np.allclose(inv, transp) # Plotting #plot_points(R) fig = plt.figure() ax = fig.add_subplot(111) x = [0, 1] y = [0, 0] plt.scatter(x, y, s=100, marker='o') #plt.show() # Test if modern-robotics library is installed properly se3mat = [[0.0, 0.0, 0.0, 0.0], [0, 0, -1.5708, 2.3562], [0, 1.5708, 0, 2.3562], [0, 0, 0, 0]] print(se3mat) T = modern_robotics.MatrixExp6(se3mat) print(T) #assert np.allclose(T, [[1, 0, 0, 0], [0, 0.0, -1, 0], [0, 1, 0, 3], [0, 0, 0, 9]])
print("Ex7, va:") Tsa = mr.Adjoint(Tas) va = np.dot(Tsa, vs) print(va) # [1,-3,-2,-3,-1,5] print("Ex8, theta:") MatLogTsa = mr.MatrixLog6(Tsa) vec = mr.so3ToVec(MatLogTsa) theta = mr.AxisAng6(vec)[-1] print(theta) # 2.094395102393196 print("Ex9, Matrix exponential:") se3 = mr.VecTose3(stheta) MatExp = mr.MatrixExp6(se3) print(MatExp) # [[-0.61727288,-0.70368982,0.35184491,1.05553472],[0.70368982,-0.2938183,0.64690915,1.94072745],[-0.35184491,0.64690915,0.67654542,-0.97036373],[0,0,0,1]] print("Ex10, fb:") Tbs = mr.Adjoint(Tbs) Tsb = np.transpose(Tbs) fb = np.dot(Tsb, fb) print(fb) # [-1,0,-4,2,0,-1] print("Ex11, TransInv:") Tinv = mr.TransInv(T) print(Tinv) # [[0,1,0,0],[-1,0,0,3],[0,0,1,-1],[0,0,0,1]]
def NextState(thetalist, dthetalist, grip2, delta_t): """ Calculates upcoming robot configuration thetalist:robot configuration dthetalist: velocity of each robot joint/wheel grip2: gripper status (0 or 1) delta_t: time-step i: increment of how many states have been calculated chassis__phi ... W4__: new configuration """ global i chassis_phi, chassis_x, chassis_y, J1, J2, J3, J4, J5, W1, W2, W3, W4, grip3 = thetalist[ 0] #Sets a maximum speed for thing in range(len(dthetalist[0])): if dthetalist[0][thing] > 9: dthetalist[0][thing] = 9 norm = np.linalg.norm(dthetalist[0]) """ if norm > 15: dthetalist[0] = dthetalist[0] / norm * 15 """ J1_, J2_, J3_, J4_, J5_, W1_, W2_, W3_, W4_ = dthetalist[0] Ts_b = np.array([[np.cos(chassis_phi), -np.sin(chassis_phi), 0, chassis_x], [np.sin(chassis_phi), np.cos(chassis_phi), 0, chassis_y], [0, 0, 1, 0.0963], [0, 0, 0, 1]]) i += 1 print(i) print( " of 2399 (If 2399 is exceeded: stop, delete generated files and try again. Won't happen again)" ) d_th = np.array([[W1_], [W2_], [W3_], [W4_]]) * delta_t Vb = np.matmul(config_, d_th) #config_.dot(d_th) Vb_6 = np.array([[0, 0, Vb[0, 0], Vb[1, 0], Vb[2, 0], 0]]).T Tb_b_pre = mr.VecTose3(Vb_6.reshape(-1)) Tb_b = mr.MatrixExp6(Tb_b_pre) Tb = np.matmul(Ts_b, Tb_b) #Ts_b.dot(Tb_b) Tb00 = Tb[0][0] Tb03 = Tb[0][3] Tb13 = Tb[1][3] #if type(Tb00) == np.ndarray: # Tb00 = Tb00.astype(float) # Tb03 = (Tb[0][3]).astype(float) # Tb13 = (Tb[1][3]).astype(float) chassis__phi = np.arccos(Tb00) chassis__x = Tb03 chassis__y = Tb13 J1__ = J1 + J1_ * delta_t J2__ = J2 + J2_ * delta_t J3__ = J3 + J3_ * delta_t J4__ = J4 + J4_ * delta_t J5__ = J5 + J5_ * delta_t W1__ = W1 + W1_ * delta_t W2__ = W2 + W2_ * delta_t W3__ = W3 + W3_ * delta_t W4__ = W4 + W4_ * delta_t the_file.append([ chassis__phi, chassis__x, chassis__y, J1__, J2__, J3__, J4__, J5__, W1__, W2__, W3__, W4__, grip2 ]) return chassis__phi, chassis__x, chassis__y, J1__, J2__, J3__, J4__, J5__, W1__, W2__, W3__, W4__, grip2
##V_a=adj_as*V_s Adt_as = Mr.Adjoint(T_as) V_s = np.array([[3, 2, 1, -1, -2, -3]]).T V_a = np.matmul(Adt_as, V_s) print(V_a) ###############Question #8########### print("Question #8\n") MatrixLog_T_sa = Mr.MatrixLog6(T_sa) MatrixLog_T_sa_vec = Mr.se3ToVec(MatrixLog_T_sa) theta = Mr.AxisAng6(MatrixLog_T_sa_vec)[1] print(theta) ###############Question #9########### print("Question #9\n") V = np.array([[0, 1, 2, 3, 0, 0]]).T S = Mr.VecTose3(V) Matrix_exponential = Mr.MatrixExp6(S) print(np.around(Matrix_exponential, 2)) ###############Question #10########### print("Question #10\n") F_b = np.array([1, 0, 0, 2, 1, 0]) Ad_T_bs = Mr.Adjoint(T_bs) Ad_T_bs_trans = np.transpose(Ad_T_bs) F_s = np.dot(Ad_T_bs_trans, F_b) print("\nQuestion 10:\n", np.array2string(F_s, separator=',')) # question 10 ###############Question #11########### print("Question #11\n") T = np.array([[0, -1, 0, 3], [1, 0, 0, 0], [0, 0, 1, 1], [0, 0, 0, 1]]) invT = Mr.TransInv(T) print(invT) ###############Question #12########### print("Question #12\n")
def robo_plot(thetalist): S = np.array([[0, 0 ,1, 0, 0, 0], [0, 1, 0, -0.505, 0, 0.150], [0, 1, 0, -1.265, 0, 0.150], [1, 0, 0, 0, 1.465, 0], [0, 1, 0, -1.465, 0, 0.945], [1, 0, 0, 0, 1.465, 0]]).T M1 = np.identity(4) M2 = np.array([[1, 0, 0, 0.150], [0, 1, 0, 0], [0, 0, 1, 0.505], [0, 0, 0, 1]]) M3 = np.array([[1, 0, 0, 0.150], [0, 1, 0, 0], [0, 0, 1, 0.760+0.505], [0, 0, 0, 1]]) M4 = np.array([[1, 0, 0, 0.150+0.238], [0, 1, 0, 0], [0, 0, 1, 0.505+0.760+0.200], [0, 0, 0, 1]]) M5 = np.array([[1, 0, 0, 0.150+0.238+0.557], [0, 1, 0, 0], [0, 0, 1, 0.505+0.760+0.200], [0, 0, 0, 1]]) M6 = np.array([[1, 0, 0, 0.150+0.238+0.557+0.1], [0, 1, 0, 0], [0, 0, 1, 0.505+0.760+0.200], [0, 0, 0, 1]]) M7 = np.array([[1, 0, 0, 0.150+0.238+0.557+0.1], [0, 1, 0, 0], [0, 0, 1, 0.505+0.760+0.200], [0, 0, 0, 1]]) joint_frames = [M1, M2, M3, M4, M5, M6, M7] #M1 - M& is a frame attached to each joint, M7 is the end effector frame trans_mat = [M1, M2, M3, M4, M5, M6, M7] exp_trans = np.identity(4) for i in range(len(S)): trans_mat[i] = exp_trans@joint_frames[i] exp_trans = [email protected](mr.VecTose3(S[:,i]*thetalist[i])) if i == range(len(S))[-1]: trans_mat[i+1] = exp_trans@joint_frames[i+1] Tr = np.radians(54) T = np.array([[np.cos(Tr), 0, np.sin(Tr), 0.450], #The transformation matrix between the end frame and the tool-tip [0, 1 ,0, 0], [-np.sin(Tr), 0, np.cos(Tr), -0.084], [0, 0, 0, 1]]) trans_mat[-1] = trans_mat[-1]@T #Garther the x,y,z coorinates in arrays x_list = np.zeros(len(trans_mat)) y_list = np.zeros(len(trans_mat)) z_list = np.zeros(len(trans_mat)) for i in range(len(trans_mat)): x_list[i] = trans_mat[i][0][3] y_list[i] = trans_mat[i][1][3] z_list[i] = trans_mat[i][2][3] #Plot the points in a 3d-coordinate system ax = plt.axes(projection='3d') plt.title('YASKAWA Motoman GP25') ax.plot(x_list, y_list ,z_list ,'-k') #Plot the robot links ax.plot(x_list[0:len(x_list)-1], y_list[0:len(y_list)-1], z_list[0:len(z_list)-1 ], 'c.') #Plot the robot joints color_list= ['-b', '-r', '-g'] c_scale = 0.3 #Used to scale the size of the coordinate axis I = np.identity(3)*c_scale for i in range(3): #Plots coordinate axis for the base frame and end effector frame ax.plot([0, I[0][i]], [0, I[1][i]], [0, I[2][i]], color_list[i]) ax.plot([x_list[-1], x_list[-1] + trans_mat[-1][0][i]*c_scale], [y_list[-1], y_list[-1] + trans_mat[-1][1][i]*c_scale], [z_list[-1], z_list[-1] + trans_mat[-1][2][i]*c_scale], color_list[i]) ax.set_xlabel('X') ax.set_ylabel('Y') ax.set_zlabel('Z') ax.set_xlim3d(-2.1,2.1) ax.set_ylim3d(-2.1,2.1) ax.set_zlim3d(0,2.4) plt.show()
def NextState(configuration, control, timeStep, velocityLimits=[1000 for x in range(12)]): """Computes the configuration of youBot after a timeStep :param configuration: A 12-vector representing the current configuration of the robot. 3 variables for the chassis configuration (theta, x, y), 5 variables for the arm configuration (J1, J2, J3, J4, J5), and 4 variables for the wheel angles (W1, W2, W3, W4) :param control: A 9-vector of controls indicating the arm joint speeds omega (5 variables) wheel speeds u (4 variables) :param timeStep: A timestep delta_t :param velocityLimits (optional): A positive real value vector indicating the maximum angular speed of the arm joints and the wheels. omega (5 variables) wheel speeds u (4 variables) If a value is not provided, default will be set to 1000 (Approx. no velocity limit) :return nextConfiguration: A 12-vector representing the configuration of the robot time delta_t later 3 variables for the chassis configuration (theta, x, y), 5 variables for the arm configuration (J1, J2, J3, J4, J5), and 4 variables for the wheel angles (W1, W2, W3, W4) The function NextState is based on a simple first-order Euler step, i.e., new arm joint angles = (old arm joint angles) + (joint speeds) * delta_t new wheel angles = (old wheel angles) + (wheel speeds) * delta_t new chassis configuration is obtained from odometry, as described in Chapter 13.4 Note: All measurements are in meters, radians and seconds Example Input: Output: """ ## Limiting control velocities velocityLimiter(control, velocityLimits) ## Calculating arm configuration armConfig = np.array(configuration[3:8]) armControl = np.array(control[0:5]) armConfig = armConfig + armControl * timeStep ## Calculating wheel configuration wheelConfig = np.array(configuration[8:12]) wheelControl = np.array(control[5:9]) wheelConfig = wheelConfig + wheelControl * timeStep ## Calculating robot configuration theta, x, y = configuration[0:3] T = kinematic_model.youBot_Tbase(theta, x, y) # u = H * V (U : wheel speeds, V : body twist) # Using Eq. 13.10 in Modern Robotics book, H = kinematic_model.H V = np.dot(np.linalg.pinv(H), wheelControl) V6 = np.array([0, 0, V[0], V[1], V[2], 0]) V6_se3mat = mr.VecTose3(V6) T_delta = mr.MatrixExp6(V6_se3mat * timeStep) T = np.dot(T, T_delta) baseConfig = kinematic_model.baseConfiguration(T) ## Updating configuration configuration = np.concatenate((baseConfig, armConfig, wheelConfig)) ## Rounding off values to 8 decimal places configuration = np.around(configuration, 8) return configuration
import numpy as np import math Tsa = [[0, -1, 0, 0], [0, 0, -1, 0], [1, 0, 0, 1], [0, 0, 0, 1]] Tsb = [[1, 0, 0, 0], [0, 0, 1, 2], [0, -1, 0, 0], [0, 0, 0, 1]] Tas = [[0, 0, 1, -1], [-1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 0, 1]] Tbs = [[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, -2], [0, 0, 0, 1]] Vs = np.array([3, 2, 1, -1, -2, -3]) AdjointTas = mr.Adjoint(Tas) AdjointTbsTranspose = np.transpose(mr.Adjoint(Tbs)) Fb = np.array([1, 0, 0, 2, 1, 0]) T = np.array([[0, -1, 0, 3], [1, 0, 0, 0], [0, 0, 1, 1], [0, 0, 0, 1]]) V = np.array([1, 0, 0, 0, 2, 3]) q = np.array([0, 0, 2]) h = 1 s = np.array([1, 0, 0]) ScrewAxis = mr.ScrewToAxis(q, s, h) matExponent14 = np.array([[0, -1.5708, 0, 2.3562], [1.5708, 0, 0, -2.3562], [0, 0, 0, 1], [0, 0, 0, 0]]) T14 = mr.MatrixExp6(matExponent14) T15 = np.array([[0, -1, 0, 3], [1, 0, 0, 0], [0, 0, 1, 1], [0, 0, 0, 1]]) matExponent15 = mr.MatrixLog6(T15) print matExponent15
import modern_robotics as mr #Q4 H0 = np.array([[-5, 1, -1], [5, 1, 1], [5, 1, -1], [-5, 1, 1]]) u = np.array([-1.18, 0.68, 0.02, -0.52]) Hinverse = np.linalg.pinv(H0) V = np.dot(Hinverse, u) # print V #Q5 V6 = np.array([0, 0, V[0], V[1], V[2], 0]) se3mat = mr.VecTose3(V6) T = mr.MatrixExp6(se3mat) R, p = mr.TransToRp(T) so3mat = mr.MatrixLog3(R) omega = mr.so3ToVec(so3mat) # print p # print omega #Q06 F = np.array([[0, 0], [0, 0], [-0.25, 0.25], [0.25, 0.25], [0, 0], [0, 0]]) # Finding Tbe omega = np.array([0, 0, math.pi / 2]) p = np.array([2, 3, 0]) so3mat = mr.VecToso3(omega) R = mr.MatrixExp3(so3mat) Tbe = mr.RpToTrans(R, p) Teb = np.linalg.inv(Tbe)
def p9(): print('P9') Stheta = np.array([0, 1, 2, 3, 0, 0]) se3mat = mr.VecTose3(Stheta) result = mr.MatrixExp6(se3mat) print(result)
def NextState(self, config, controls, timestep): ''' Input : Chasis configuration, arm and wheel controls, timestep and dimensions of the robot config : { chasis: [phi,x,y], arm: [0,0,0,0,0], wheel : [0,0,0,0] } controls : {arm : [joint_1_speed, 2,3,4], wheel : [wheel 1 speed, 2,3,4]} timestep : delta time Output : New configuration after the controls for timestep new_config : { chasis: [phi,x,y], arm: [0,0,0,0,0], wheel : [0,0,0,0] } ''' new_config = {} new_config["chasis"] = [0,0,0] new_config["arm"] = [0,0,0,0,0] new_config["wheel"] = [0,0,0,0] # New wheel configuration del_theta_wheel = [0]*len(new_config["wheel"]) del_theta_joint = [0]*len(new_config["arm"]) for wheel_no in range(len(new_config["wheel"])): # Checking if wheel speed is off limits if fabs(controls["wheel"][wheel_no]) > fabs(self.limit_wheel[wheel_no]): print ("Wheel velocity exceeded for wheel ",wheel_no + 1, controls["wheel"][wheel_no]) controls["wheel"][wheel_no] = copysign(self.limit_wheel[wheel_no],controls["wheel"][wheel_no]) del_theta_wheel[wheel_no] = controls["wheel"][wheel_no] * timestep # delta theta to be multiplied with F6 to find Vb6 new_config["wheel"][wheel_no] = config["wheel"][wheel_no] + del_theta_wheel[wheel_no] # New joint angles for joint_no in range(len(new_config["arm"])): # Checking if arm joint speed is off limits if fabs(controls["arm"][joint_no]) > fabs(self.limit_arm[joint_no]): print ("Joint velocity exceeded for Joint ",joint_no + 1, controls["arm"][joint_no]) controls["arm"][joint_no] = copysign(self.limit_arm[joint_no],controls["arm"][joint_no]) del_theta_joint[joint_no] = controls["arm"][joint_no] * timestep new_config["arm"][joint_no] = config["arm"][joint_no] + del_theta_joint[joint_no] phi = config["chasis"][0] x = config["chasis"][1] y = config["chasis"][2] Tsb = np.array([[cos(phi),sin(phi),0,0],[-sin(phi),cos(phi),0,0],[0,0,1,0],[x,y,self.height,1]]).T # Finding skew symmetric matrix and integrate to find the delta transformation matrix Vb6 = self.F6.dot(del_theta_wheel) # Multiplied with timestep as Vb6 is the twist for unit time se3mat = mr.VecTose3(Vb6) Tbk_b1k = mr.MatrixExp6(se3mat) # New Position of the bot wrt space frame Tsb1k = Tsb.dot(Tbk_b1k) # theta = acos(Tsb1k[0,0]) # 0th element is cos(phi)..so inverse gives phi theta = atan2(Tsb1k[1,0],Tsb1k[0,0]) # phi in range of -pi to pi new_config["chasis"][0] = theta new_config["chasis"][1] = Tsb1k[0,-1] new_config["chasis"][2] = Tsb1k[1,-1] # print config, controls, limits return new_config # chasis, arm, wheel
def TrajectoryGenerator(Xstart, Tsci, Tscf, Tceg, Tces, k): """ Generates end-effector trajectory between 9 points (8 transitions) and adds them to a csv file The trajectory from the first to second standoff position is a screw instead of a Cartesian. Xstart: initial configuration of end-effector in reference trajectory (Tsei) Tsci: cube's initial configuration Tscf: cube's final configuration Tceg: end-effector position relative to cube when grasping Tces: end-effector position above the cube before and after grasping k: number of reference trajectory configurations per centisecond """ Tf = 3 method = 5 Xend = np.matmul(Tsci, Tces) #first standoff FSL = np.matmul(Tsci, Tceg) #first standoff lowered SS = np.matmul(Tscf, Tces) #second standoff SSL = np.matmul(Tscf, Tceg) #second standoff lowered Xtheend = np.matmul(Tscf, Tces) N = Tf * 100 * k N = int(N) timegap = Tf / (N - 1.0) traj = [[None]] * N Rstart, pstart = mr.TransToRp(Xstart) Rend, pend = mr.TransToRp(Xend) for i in range(N): if method == 3: s = mr.CubicTimeScaling(Tf, timegap * i) else: s = mr.QuinticTimeScaling(Tf, timegap * i) traj[i] \ = np.r_[np.c_[np.dot(Rstart, \ mr.MatrixExp3(mr.MatrixLog3(np.dot(np.array(Rstart).T,Rend)) * s)), \ s * np.array(pend) + (1 - s) * np.array(pstart)], \ [[0, 0, 0, 1]]] row = [] for p in range(len(traj[i]) - 1): for l in range(0, 3): row.append(traj[i][p][l]) for m in range(len(traj[i]) - 1): for n in range(0, 4): if n == 3: row.append(traj[i][m][n]) row.append(0) with open('penultimate.csv', 'a') as csvFile: writer = csv.writer(csvFile) writer.writerow(row) csvFile.close() N = int(N) timegap = Tf / (N - 1.0) traj = [[None]] * N RstartFSL, pstartFSL = mr.TransToRp(Xend) RendFSL, pendFSL = mr.TransToRp(FSL) for i in range(N): if method == 3: s = mr.CubicTimeScaling(Tf, timegap * i) else: s = mr.QuinticTimeScaling(Tf, timegap * i) traj[i] \ = np.r_[np.c_[np.dot(RstartFSL, \ mr.MatrixExp3(mr.MatrixLog3(np.dot(np.array(RstartFSL).T,RendFSL)) * s)), \ s * np.array(pendFSL) + (1 - s) * np.array(pstartFSL)], \ [[0, 0, 0, 1]]] row = [] for q in range(len(traj[i]) - 1): for r in range(0, 3): row.append(traj[i][q][r]) for s in range(len(traj[i]) - 1): for t in range(0, 4): if t == 3: row.append(traj[i][s][t]) row.append(0) with open('penultimate.csv', 'a') as csvFile: writer = csv.writer(csvFile) writer.writerow(row) csvFile.close() N = int(N) timegap = Tf / (N - 1.0) traj = [[None]] * N RstartFC, pstartFC = mr.TransToRp(FSL) #FC = FIRST CLOSE RendFC, pendFC = mr.TransToRp(FSL) for i in range(N): if method == 3: s = mr.CubicTimeScaling(Tf, timegap * i) else: s = mr.QuinticTimeScaling(Tf, timegap * i) traj[i] \ = np.r_[np.c_[np.dot(RstartFC, \ mr.MatrixExp3(mr.MatrixLog3(np.dot(np.array(RstartFC).T,RendFC)) * s)), \ s * np.array(pendFC) + (1 - s) * np.array(pstartFC)], \ [[0, 0, 0, 1]]] row = [] for u in range(len(traj[i]) - 1): for v in range(0, 3): row.append(traj[i][u][v]) for w in range(len(traj[i]) - 1): for x in range(0, 4): if x == 3: row.append(traj[i][w][x]) row.append(1) with open('penultimate.csv', 'a') as csvFile: writer = csv.writer(csvFile) writer.writerow(row) csvFile.close() N = int(N) timegap = Tf / (N - 1.0) traj = [[None]] * N RstartFrise, pstartFrise = mr.TransToRp(FSL) #Frise = first rise RendFrise, pendFrise = mr.TransToRp(Xend) for i in range(N): if method == 3: s = mr.CubicTimeScaling(Tf, timegap * i) else: s = mr.QuinticTimeScaling(Tf, timegap * i) traj[i] \ = np.r_[np.c_[np.dot(RstartFrise, \ mr.MatrixExp3(mr.MatrixLog3(np.dot(np.array(RstartFrise).T,RendFrise)) * s)), \ s * np.array(pendFrise) + (1 - s) * np.array(pstartFrise)], \ [[0, 0, 0, 1]]] row = [] for y in range(len(traj[i]) - 1): for z in range(0, 3): row.append(traj[i][y][z]) for aa in range(len(traj[i]) - 1): for ab in range(0, 4): if ab == 3: row.append(traj[i][aa][ab]) row.append(1) with open('penultimate.csv', 'a') as csvFile: writer = csv.writer(csvFile) writer.writerow(row) csvFile.close() """ N = int(N) timegap = Tf / (N - 1.0) traj = [[None]] * N RstartSS, pstartSS = mr.TransToRp(Xend) #Frise = first rise RendSS, pendSS = mr.TransToRp(SS) for i in range(N): if method == 3: s = mr.CubicTimeScaling(Tf, timegap * i) else: s = mr.QuinticTimeScaling(Tf, timegap * i) traj[i] \ = np.r_[np.c_[np.dot(RstartSS, \ mr.MatrixExp3(mr.MatrixLog3(np.dot(np.array(RstartSS).T,RendSS)) * s)), \ s * np.array(pendSS) + (1 - s) * np.array(pstartSS)], \ [[0, 0, 0, 1]]] """ N = int(N) timegap = Tf / (N - 1.0) traj = [[None]] * N for i in range(N): if method == 3: s = mr.CubicTimeScaling(Tf, timegap * i) else: s = mr.QuinticTimeScaling(Tf, timegap * i) traj[i] \ = np.dot(Xend, mr.MatrixExp6(mr.MatrixLog6(np.dot(mr.TransInv(Xend), \ SS)) * s)) row = [] for ac in range(len(traj[i]) - 1): for ad in range(0, 3): row.append(traj[i][ac][ad]) for ae in range(len(traj[i]) - 1): for af in range(0, 4): if af == 3: row.append(traj[i][ae][af]) row.append(1) with open('penultimate.csv', 'a') as csvFile: writer = csv.writer(csvFile) writer.writerow(row) csvFile.close() N = int(N) timegap = Tf / (N - 1.0) traj = [[None]] * N RstartSSL, pstartSSL = mr.TransToRp(SS) #Frise = first rise RendSSL, pendSSL = mr.TransToRp(SSL) for i in range(N): if method == 3: s = mr.CubicTimeScaling(Tf, timegap * i) else: s = mr.QuinticTimeScaling(Tf, timegap * i) traj[i] \ = np.r_[np.c_[np.dot(RstartSSL, \ mr.MatrixExp3(mr.MatrixLog3(np.dot(np.array(RstartSSL).T,RendSSL)) * s)), \ s * np.array(pendSSL) + (1 - s) * np.array(pstartSSL)], \ [[0, 0, 0, 1]]] row = [] for ag in range(len(traj[i]) - 1): for ah in range(0, 3): row.append(traj[i][ag][ah]) for ai in range(len(traj[i]) - 1): for aj in range(0, 4): if aj == 3: row.append(traj[i][ai][aj]) row.append(1) with open('penultimate.csv', 'a') as csvFile: writer = csv.writer(csvFile) writer.writerow(row) csvFile.close() N = int(N) timegap = Tf / (N - 1.0) traj = [[None]] * N RstartSSU, pstartSSU = mr.TransToRp(SSL) #Frise = first rise RendSSU, pendSSU = mr.TransToRp(SSL) for i in range(N): if method == 3: s = mr.CubicTimeScaling(Tf, timegap * i) else: s = mr.QuinticTimeScaling(Tf, timegap * i) traj[i] \ = np.r_[np.c_[np.dot(RstartSSU, \ mr.MatrixExp3(mr.MatrixLog3(np.dot(np.array(RstartSSU).T,RendSSU)) * s)), \ s * np.array(pendSSU) + (1 - s) * np.array(pstartSSU)], \ [[0, 0, 0, 1]]] row = [] for ak in range(len(traj[i]) - 1): for al in range(0, 3): row.append(traj[i][ak][al]) for am in range(len(traj[i]) - 1): for an in range(0, 4): if an == 3: row.append(traj[i][am][an]) row.append(0) with open('penultimate.csv', 'a') as csvFile: writer = csv.writer(csvFile) writer.writerow(row) csvFile.close() N = int(N) timegap = Tf / (N - 1.0) traj = [[None]] * N RstartSSD, pstartSSD = mr.TransToRp(SSL) #Frise = first rise RendSSD, pendSSD = mr.TransToRp(Xtheend) for i in range(N): if method == 3: s = mr.CubicTimeScaling(Tf, timegap * i) else: s = mr.QuinticTimeScaling(Tf, timegap * i) traj[i] \ = np.r_[np.c_[np.dot(RstartSSD, \ mr.MatrixExp3(mr.MatrixLog3(np.dot(np.array(RstartSSD).T,RendSSD)) * s)), \ s * np.array(pendSSD) + (1 - s) * np.array(pstartSSD)], \ [[0, 0, 0, 1]]] row = [] for ao in range(len(traj[i]) - 1): for ap in range(0, 3): row.append(traj[i][ao][ap]) for aq in range(len(traj[i]) - 1): for ar in range(0, 4): if ar == 3: row.append(traj[i][aq][ar]) row.append(0) with open('penultimate.csv', 'a') as csvFile: writer = csv.writer(csvFile) writer.writerow(row) csvFile.close() return traj #maybe no return statement needed