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 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 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
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]]
import math 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)
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 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)
def p12(): print('P12') V = np.array([1, 0, 0, 0, 2, 3]) result = mr.VecTose3(V) print(result)
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(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
print("Question #7\n") ##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###########
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()
cos(y_rotation), 0], [0, 0, 0, 1]]) Tsc_goal = np.array([[0, 1, 0, 4.7510e-01], [-1, 0, 0, -1.0750e+00], [0, 0, 1, 0.025], [0, 0, 0, 1]]) B1, B2, B3, B4, B5 = [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] Blist = np.array(([B1, B2, B3, B4, B5])).T #print(Blist) _B1_ = mr.VecTose3([0, 0, 1, 0, 0.033, 0]) _B2_ = mr.VecTose3([0, -1, 0, -0.5076, 0, 0]) _B3_ = mr.VecTose3([0, -1, 0, -0.3526, 0, 0]) _B4_ = mr.VecTose3([0, -1, 0, -0.2176, 0, 0]) _B5_ = mr.VecTose3([0, 0, 1, 0, 0, 0]) r = 0.0475 w = 0.3 / 2 l = 0.47 / 2 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]]) # npad is a tuple of (n_before, n_after) for each dimension npad = ((2, 1), (0, 0)) F6 = np.pad(F, pad_width=npad, mode='constant', constant_values=0)