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
