增加Python控制youBot的Pick and Place的Demo

7_Demo_youBotPickAndPlace/.gitignore

@@ -0,0 +1 @@
+__pycache__

7_Demo_youBotPickAndPlace/README.md

@@ -0,0 +1,45 @@
+# 说明
+
+---
+
+在使用此源代码的时候，建议读者配合对应的文章教程进行阅读。文章链接详见知乎专栏“AI与机器人”或者微信公众号“博士的沙漏”中的第八期自学笔记。
+
+本项目参考了美国西北大学的Kevin M. Lynch教授和韩国首尔国立大学的Frank C. Park教授共同编撰的《Modern Robotics: Mechanics, Planning and Control》一书，强烈建议有兴趣的读者阅读此书。
+
+TrajectoryGenerator生成的reference trajectory一共有13个变量，分别表示
+
+| 编号 |  1   |  2   |  3   |  4   |  5   |  6   |  7   |  8   |  9   |  10  |  11  |  12  |      13       |
+| :--: | :--: | :--: | :--: | :--: | :--: | :--: | :--: | :--: | :--: | :--: | :--: | :--: | :-----------: |
+| 数值 | phi  |  x   |  y   |  J1  |  J2  |  J3  |  J4  |  J5  |  w1  |  w2  |  w3  |  w4  | gripper state |
+
+其中，
+
+- x、 y、phi 分别表示World_X_Joint、World_Y_Joint、World_Th_Joint的坐标和偏转角度；
+- J1， $\dots$， J5 分别表示youBot的机械臂上的5个关节角度；
+- w1，$\dots$， w4分别表示front left, front right, rail right, rail left四个轮子的位置（旋转角度），如下图所示；
+- gripper state即表示gripper的状态，0表示打开，1表示闭合。
+
+注意：x、y 的单位是米，phi、J1~J5， w1~w4等角度的单位是弧度。控制Gripper的打开和闭合过程至少需要0.625秒，所以为了保证Gripper能正常完成Open/Close的动作，在编写程序控制Gripper从打开到闭合（或从闭合到打开），确保要在该位置保持0.625秒以上。
+
+youBot地盘上四个轮子的编号如图所示：
+
+![YoubotTopView](README.assets/YoubotTopView.png)
+
+分别对应w1、w2、w3、w4四个轮子旋转的角度。
+
+在VREP中，youBot模型机械臂的基本尺寸参数如图所示：
+
+<img src="README.assets/youBotParams.png" alt="youBotParams" style="zoom:80%;" />
+
+对于抓手（Gripper）来说，其尺寸参数如图所示：
+
+<img src="README.assets/GripperParams.png" alt="GripperParams" style="zoom:33%;" />
+
+其中：
+$$
+d_{1, min} = 2 \ cm\\
+d_{1, max} = 7 \ cm\\
+d_2 = 3.5 \ cm\\
+d_3 = 4.3 \ cm
+$$
+对于底盘来说，前后轮之间的距离是0.47m，左右轮之间的距离是0.3m，轮子的半径为0.0475m。

7_Demo_youBotPickAndPlace/code/FeedbackControl.py

@@ -0,0 +1,81 @@
+import numpy as np
+import modern_robotics as mr
+from JointLimits import jointLimits
+
+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]])
+    T_sb_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 = T_sb_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 J_e
+    # 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
+    T_sb = 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]])
+    T_eb = np.linalg.inv(X).dot(T_sb)
+    J_base = mr.Adjoint(T_eb).dot(F6)
+
+    # here we test joint limits
+    if jointlimit == 1:
+        jointLimits(config,J_arm)
+
+    # finally we write down J_e
+    J_e = np.hstack((J_base,J_arm))
+
+    # here we write down speeds (u,thetadot)
+    speeds = np.linalg.pinv(J_e,rcond=1e-2).dot(V)
+
+    return V, speeds, X_err

7_Demo_youBotPickAndPlace/code/JointLimits.py

@@ -0,0 +1,8 @@
+import numpy as np
+
+def jointLimits(config,J_arm):
+    if config[0,5] > -0.1:
+        J_arm[:,2] = np.array([0,0,0,0,0,0])
+    if config[0,6] > -0.1:
+        J_arm[:,3] = np.array([0,0,0,0,0,0])
+

7_Demo_youBotPickAndPlace/code/NextState.py

@@ -0,0 +1,42 @@
+import numpy as np
+
+def nextState(config,speed,delta_t,speed_max):
+    # here we limit speed
+    for j in np.arange(np.shape(speed)[1]):
+        if speed[0,j]>speed_max:
+            speed[0,j] = speed_max
+        elif speed[0,j]<-speed_max:
+            speed[0,j] = -speed_max
+
+    # here we get the new arm and wheels configurations
+    new_angle_config = config[0,3:12]+speed*delta_t
+
+    # here we start to compute the configuration of chassis using odometry
+    # step1: compute delta theta, which is the change of wheels rotation
+    delta_theta = (speed[0,5:].reshape(1,4)).T*delta_t
+
+    # step2: compute the wheels velocities
+    theta_dot = delta_theta
+
+    # step3: compute chassis planar twist Vb
+    # here we write down some necessary parameters for step3
+    r = 0.0475
+    l = 0.47/2
+    w = 0.3/2
+    Vb = (r/4)*np.array([[-1/(l+w),1/(l+w),1/(l+w),-1/(l+w)],[1,1,1,1],[-1,1,-1,1]]).dot(theta_dot)
+
+    # step4: calculate new chassis config
+    # here we write down dqb
+    if Vb[0,0] == 0:
+        dqb = np.array([[0],[Vb[1,0]],[Vb[2,0]]])
+    elif Vb[0,0] != 0:
+        dqb = np.array([[Vb[0,0]],[(Vb[1,0]*np.sin(Vb[0,0])+Vb[2,0]*(np.cos(Vb[0,0])-1))/Vb[0,0]],[(Vb[2,0]*np.sin(Vb[0,0])+Vb[1,0]*(1-np.cos(Vb[0,0])))/Vb[0,0]]])
+    # here we write down dq
+    dq = np.array([[1,0,0],[0,np.cos(config[0,0]),-np.sin(config[0,0])],[0,np.sin(config[0,0]),np.cos(config[0,0])]]).dot(dqb)
+    # here we write down new chassis config
+    new_chassis_config = config[0,0:3].reshape(1,3)+dq.reshape(1,3)
+
+    # here we put the angle and chassis configuration together
+    new_config = np.hstack((new_chassis_config,new_angle_config))
+
+    return new_config

7_Demo_youBotPickAndPlace/code/TrajectoryGenerator.py

@@ -0,0 +1,86 @@
+import numpy as np
+import modern_robotics as mr
+
+def trajectoryGenerator(T_se_initial, T_sc_initial, T_sc_final, T_ce_grasp, T_ce_standoff, k):
+    # ---------------------------------
+    # segment 1: A trajectory to move the gripper from its initial configuration to a "standoff" configuration a few cm above the block
+    # represent the frame of end-effector when standoff in space frame
+    T_se_standoff_initial = T_sc_initial.dot(T_ce_standoff)
+    # generate trajectory when approaching the standoff position in segment 1
+    T_se_segment_1 = mr.CartesianTrajectory(T_se_initial,T_se_standoff_initial,5,5/0.01,3)
+
+    # ---------------------------------
+    # segment 2: A trajectory to move the gripper down to the grasp position
+    # represent the frame of end-effecto    r in space frame in segment 2
+    T_se_grasp = T_sc_initial.dot(T_ce_grasp)
+    # generate trajectory when approaching the grasping position in segment 2
+    T_se_seg2 = mr.CartesianTrajectory(T_se_segment_1[-1], T_se_grasp, 2, 2/0.01, 3)
+    # append the trajectory of segment 2 after segment 1
+    T_se_before = np.append(T_se_segment_1, T_se_seg2, axis=0)
+
+    # ---------------------------------
+    # segment 3: Closing of the gripper
+    # append the trajectory of segment 3 by 63 times
+    for i in np.arange(64):
+        T_se_before = np.append(T_se_before,np.array([T_se_before[-1]]),axis=0)
+
+    # ---------------------------------
+    # segment 4: A trajectory to move the gripper back up to the "standoff" configuration
+    # generate trajectory when back on the standoff position in segment 4
+    T_se_segment_4 = mr.CartesianTrajectory(T_se_grasp, T_se_standoff_initial, 2, 2/0.01, 3)
+    # append the trajectory of segment 4
+    T_se_before = np.append(T_se_before,T_se_segment_4,axis=0)
+
+    # ---------------------------------
+    # segment 5: A trajectory to move the gripper to a "standoff" configuration above the final configuration
+    # generate trajectory when moving to the final standoff position in segment 5
+    T_se_standoff_final = T_sc_final.dot(T_ce_standoff)
+    T_se_segment_5 = mr.CartesianTrajectory(T_se_standoff_initial, T_se_standoff_final, 8, 8/0.01, 3)
+    # append the trajectory of segment 5
+    T_se_before = np.append(T_se_before, T_se_segment_5, axis=0)
+
+    # ---------------------------------
+    # segment 6: A trajectory to move the gripper to the final configuration of the object
+    # generate the end-effector configuration when losing
+    T_se_lose = T_sc_final.dot(T_ce_grasp)
+    # generate trajectory when moving to the final cube position in segment 6
+    T_se_segment_6 = mr.CartesianTrajectory(T_se_standoff_final, T_se_lose, 2, 2/0.01, 3)
+    # append the trajectory of segment 6
+    T_se_before = np.append(T_se_before, T_se_segment_6, axis=0)
+
+    # ---------------------------------
+    # segment 7: Opening of the gripper
+    # append the trajectory of segment 7 by 63 times
+    for i in np.arange(64):
+        T_se_before = np.append(T_se_before, np.array([T_se_before[-1]]), axis=0)
+
+    # ---------------------------------
+    # segment 8: A trajectory to move the gripper back to the "standoff" configuration
+    # generate trajectory when moving to the final standoff position in segment 8
+    T_se_segment_8 = mr.CartesianTrajectory(T_se_before[-1], T_se_standoff_final, 2, 2/0.01, 3)
+    # append the trajectory of segment 8
+    T_se_before = np.append(T_se_before, T_se_segment_8, axis=0)
+
+    # ---------------------------------
+    # generate a matrix which is n by 13
+    T_se_post = np.zeros([int(k*21/0.01+64*2),13])
+    # put the configuration, position and gripper state in matrix which is n by 13
+    for i in np.arange(int(k*21/0.01+64*2)):
+        T_se_post[i,0] = T_se_before[i,0,0]
+        T_se_post[i,1] = T_se_before[i,0,1]
+        T_se_post[i,2] = T_se_before[i,0,2]
+        T_se_post[i,3] = T_se_before[i,1,0]
+        T_se_post[i,4] = T_se_before[i,1,1]
+        T_se_post[i,5] = T_se_before[i,1,2]
+        T_se_post[i,6] = T_se_before[i,2,0]
+        T_se_post[i,7] = T_se_before[i,2,1]
+        T_se_post[i,8] = T_se_before[i,2,2]
+        T_se_post[i,9] = T_se_before[i,0,3]
+        T_se_post[i,10] = T_se_before[i,1,3]
+        T_se_post[i,11] = T_se_before[i,2,3]
+        T_se_post[i,12] = 0
+    # amend the gripper state in segment 3, 4, 5, 6
+    for i in np.arange(int(k*7/0.01), int(k*19/0.01+64)):
+        T_se_post[i, 12] = 1
+
+    return T_se_post