diff --git a/7_Demo_youBotPickAndPlace/.gitignore b/7_Demo_youBotPickAndPlace/.gitignore new file mode 100644 index 0000000..ed8ebf5 --- /dev/null +++ b/7_Demo_youBotPickAndPlace/.gitignore @@ -0,0 +1 @@ +__pycache__ \ No newline at end of file diff --git a/7_Demo_youBotPickAndPlace/README.assets/GripperParams.png b/7_Demo_youBotPickAndPlace/README.assets/GripperParams.png new file mode 100644 index 0000000..751894b Binary files /dev/null and b/7_Demo_youBotPickAndPlace/README.assets/GripperParams.png differ diff --git a/7_Demo_youBotPickAndPlace/README.assets/YoubotTopView.png b/7_Demo_youBotPickAndPlace/README.assets/YoubotTopView.png new file mode 100644 index 0000000..e969e31 Binary files /dev/null and b/7_Demo_youBotPickAndPlace/README.assets/YoubotTopView.png differ diff --git a/7_Demo_youBotPickAndPlace/README.assets/youBotParams.png b/7_Demo_youBotPickAndPlace/README.assets/youBotParams.png new file mode 100644 index 0000000..d2fe29d Binary files /dev/null and b/7_Demo_youBotPickAndPlace/README.assets/youBotParams.png differ diff --git a/7_Demo_youBotPickAndPlace/README.md b/7_Demo_youBotPickAndPlace/README.md new file mode 100644 index 0000000..282f427 --- /dev/null +++ b/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模型机械臂的基本尺寸参数如图所示： + +youBotParams + +对于抓手（Gripper）来说，其尺寸参数如图所示： + +GripperParams + +其中： +$$ +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。 diff --git a/7_Demo_youBotPickAndPlace/code/FeedbackControl.py b/7_Demo_youBotPickAndPlace/code/FeedbackControl.py new file mode 100644 index 0000000..ee5f701 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/7_Demo_youBotPickAndPlace/code/JointLimits.py b/7_Demo_youBotPickAndPlace/code/JointLimits.py new file mode 100644 index 0000000..ea3de2a --- /dev/null +++ b/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]) + \ No newline at end of file diff --git a/7_Demo_youBotPickAndPlace/code/NextState.py b/7_Demo_youBotPickAndPlace/code/NextState.py new file mode 100644 index 0000000..b3b2b8c --- /dev/null +++ b/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 \ No newline at end of file diff --git a/7_Demo_youBotPickAndPlace/code/TrajectoryGenerator.py b/7_Demo_youBotPickAndPlace/code/TrajectoryGenerator.py new file mode 100644 index 0000000..36f2a7c --- /dev/null +++ b/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 \ No newline at end of file diff --git a/7_Demo_youBotPickAndPlace/code/main.py b/7_Demo_youBotPickAndPlace/code/main.py new file mode 100644 index 0000000..1d5c046 --- /dev/null +++ b/7_Demo_youBotPickAndPlace/code/main.py @@ -0,0 +1,248 @@ +import numpy as np +from TrajectoryGenerator import trajectoryGenerator +from NextState import nextState +from FeedbackControl import feedbackControl + +import time +import sys +sys.path.append('./vrep/VREP_remoteAPIs') + +#%% VREP control function definition +''' Arm control function of youBot ''' +def VREP_armControl(vrep_sim, clientID, arm_joints_handle, desired_arm_joint_angles): + for i in range(0,5): + vrep_sim.simxSetJointPosition(clientID, arm_joints_handle[i], desired_arm_joint_angles[i], vrep_sim.simx_opmode_oneshot) + +''' Wheels control function of youBot ''' +def VREP_wheelsControl(vrep_sim, clientID, wheel_joints_handle, desired_wheel_positions): + for i in range(0,4): + vrep_sim.simxSetJointPosition(clientID, wheel_joints_handle[i], desired_wheel_positions[i], vrep_sim.simx_opmode_oneshot) + +''' World frame control function of youBot ''' +def VREP_WorldFrameControl(vrep_sim, clientID, world_joints_handle, desired_world_positions): + for i in range(0,3): + vrep_sim.simxSetJointPosition(clientID, world_joints_handle[i], desired_world_positions[i], vrep_sim.simx_opmode_oneshot) + + +#%% VREP initialization +try: + import sim as vrep_sim +except: + print ('--------------------------------------------------------------') + print ('"sim.py" could not be imported. This means very probably that') + print ('either "sim.py" or the remoteApi library could not be found.') + print ('Make sure both are in the same folder as this file,') + print ('or appropriately adjust the file "sim.py"') + print ('--------------------------------------------------------------') + print ('') + +print ('Program started') +vrep_sim.simxFinish(-1) # just in case, close all opened connections +clientID = vrep_sim.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to CoppeliaSim +if clientID != -1: + print ('Connected to remote API server') + + return_code, youBot_handle = vrep_sim.simxGetObjectHandle(clientID, 'youBot', vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object youBot ok.') + + return_code, youBot_dummy_handle = vrep_sim.simxGetObjectHandle(clientID, 'youBotDummy', vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object youBotDummy ok.') + + # Prepare initial values for four wheels + wheel_joints_handle = [-1,-1,-1,-1]; # front left, rear left, rear right, front right + return_code, wheel_joints_handle[0] = vrep_sim.simxGetObjectHandle(clientID, 'rollingJoint_fl', vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object youBot rollingJoint_rl ok.') + + return_code, wheel_joints_handle[1] = vrep_sim.simxGetObjectHandle(clientID, 'rollingJoint_fr', vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object youBot rollingJoint_fl ok.') + + return_code, wheel_joints_handle[2] = vrep_sim.simxGetObjectHandle(clientID, 'rollingJoint_rr', vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object youBot rollingJoint_fr ok.') + + return_code, wheel_joints_handle[3] = vrep_sim.simxGetObjectHandle(clientID, 'rollingJoint_rl', vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object youBot rollingJoint_rr ok.') + + # Prepare initial values for five arm joints + arm_joints_handle = [-1,-1,-1,-1,-1] + for i in range(0,5): + return_code, arm_joints_handle[i] = vrep_sim.simxGetObjectHandle(clientID, 'Joint' + str(i+1), vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object arm joint ' + str(i+1) + ' ok.') + + return_code, gripper_joint_1_handle = vrep_sim.simxGetObjectHandle(clientID, 'youBotGripperJoint1', vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object gripper joint 1 ok') + + return_code, gripper_joint_2_handle = vrep_sim.simxGetObjectHandle(clientID, 'youBotGripperJoint2', vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object gripper joint 2 ok') + + return_code, gripper_tip_handle = vrep_sim.simxGetObjectHandle(clientID, 'youBot_positionTip', vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object gripper position tip ok') + + # Desired joint positions for initialization + desired_arm_joint_angles = [0, 0, 0, 0, 0] + + # Initialization all arm joints + for i in range(0,5): + vrep_sim.simxSetJointPosition(clientID, arm_joints_handle[i], desired_arm_joint_angles[i], vrep_sim.simx_opmode_blocking) + + # Get world joints handles + world_joints_handle = [-1, -1, -1] + return_code, world_joints_handle[0] = vrep_sim.simxGetObjectHandle(clientID, 'World_X_Joint', vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object world joint X ok.') + + return_code, world_joints_handle[1] = vrep_sim.simxGetObjectHandle(clientID, 'World_Y_Joint', vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object world joint Y ok.') + + return_code, world_joints_handle[2] = vrep_sim.simxGetObjectHandle(clientID, 'World_Th_Joint', vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object world joint Th ok.') + + # Get initial and goal cube handles + return_code, cube_1_handle = vrep_sim.simxGetObjectHandle(clientID, 'Cube_1', vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object cube_1 ok.') + + return_code, cube_2_handle = vrep_sim.simxGetObjectHandle(clientID, 'Cube_2', vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object cube_2 ok.') + + return_code, cube_goal_handle = vrep_sim.simxGetObjectHandle(clientID, 'Cube_goal', vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object cube_goal_1 ok.') +else: + print ('Failed connecting to remote API server') + + +#%% ---------------------- generate trajectory and control the robot --------------------------- +# Get initial and goal cube positions +_, cube_1_position = vrep_sim.simxGetObjectPosition(clientID, cube_1_handle, -1, vrep_sim.simx_opmode_blocking) +_, cube_2_position = vrep_sim.simxGetObjectPosition(clientID, cube_2_handle, -1, vrep_sim.simx_opmode_blocking) +_, cube_goal_position = vrep_sim.simxGetObjectPosition(clientID, cube_goal_handle, -1, vrep_sim.simx_opmode_blocking) + + +for i in range (0,2): + # Get youBot position + _, youBot_position = vrep_sim.simxGetObjectPosition(clientID, youBot_handle, -1, vrep_sim.simx_opmode_blocking) + _, youBot_x = vrep_sim.simxGetJointPosition(clientID, world_joints_handle[0], vrep_sim.simx_opmode_blocking) + _, youBot_y = vrep_sim.simxGetJointPosition(clientID, world_joints_handle[1], vrep_sim.simx_opmode_blocking) + _, youBot_phi = vrep_sim.simxGetJointPosition(clientID, world_joints_handle[2], vrep_sim.simx_opmode_blocking) + + # Get youBot arm joint angles + youBot_arm_q = [0, 0, 0, 0, 0] + for j in range(0, 5): + _, youBot_arm_q[j] = vrep_sim.simxGetJointPosition(clientID, arm_joints_handle[j], vrep_sim.simx_opmode_blocking) + + # Get youBot wheel angles + youBot_wheel_angles = [0, 0, 0, 0] + for j in range(0, 4): + _, youBot_wheel_angles[j] = vrep_sim.simxGetJointPosition(clientID, wheel_joints_handle[j], vrep_sim.simx_opmode_blocking) + + # Get youBot gripper position + _, youBot_gripper_position = vrep_sim.simxGetObjectPosition(clientID, gripper_tip_handle, -1, vrep_sim.simx_opmode_blocking) + + # Initialize the robot's configuration + # phi x y J1 J2 J3 J4 J5 w1 w2 w3 w4 gripper + config_initial = np.array([youBot_phi, youBot_x, youBot_y, youBot_arm_q[0], youBot_arm_q[1], youBot_arm_q[2], youBot_arm_q[3], youBot_arm_q[4], youBot_wheel_angles[0], youBot_wheel_angles[1], youBot_wheel_angles[2], youBot_wheel_angles[3], 0]).reshape(1,13) + + # Generate trajectory + T_se_initial = np.array([[0,0,1,youBot_gripper_position[0]],[0,1,0,youBot_gripper_position[1]],[-1,0,0,youBot_gripper_position[2]],[0,0,0,1]]) + if i == 0: + T_sc_initial = np.array([[1,0,0,cube_1_position[0]],[0,1,0,cube_1_position[1]],[0,0,1,cube_1_position[2]],[0,0,0,1]]) + T_sc_final = np.array([[0,1,0,cube_goal_position[0]],[-1,0,0,cube_goal_position[1]],[0,0,1, cube_goal_position[2]],[0,0,0,1]]) + else: + T_sc_initial = np.array([[1,0,0,cube_2_position[0]],[0,1,0,cube_2_position[1]],[0,0,1,cube_2_position[2]],[0,0,0,1]]) + T_sc_final = np.array([[0,1,0,cube_goal_position[0]],[-1,0,0,cube_goal_position[1]],[0,0,1, cube_goal_position[2] + 0.05],[0,0,0,1]]) + gripper_pose = np.pi/3 # change orientation of the gripper, default value is pi/4 + T_ce_grasp = np.array([[-np.sin(gripper_pose), 0, np.cos(gripper_pose),0],[0,1,0,0],[-np.cos(gripper_pose), 0, -np.sin(gripper_pose),0],[0,0,0,1]]) + T_ce_standoff = np.array([[-np.sin(gripper_pose), 0, np.cos(gripper_pose),0],[0,1,0,0],[-np.cos(gripper_pose), 0, -np.sin(gripper_pose),0.1],[0,0,0,1]]) + + k = 1 + T_se_post = trajectoryGenerator(T_se_initial,T_sc_initial,T_sc_final,T_ce_grasp,T_ce_standoff,k) + + # set to use joint limits + joint_limit = 0 + + # set Kp, Ki and dt + Kp = 2.2 + Ki = 0 + dt = 0.01 + + #%% ------------------------------------- main loop -------------------------------------- + desired_wheel_positions = [0,0,0,0] + desired_world_positions = [0,0,0] + reference_trajectory = [] + reference_trajectory.append(config_initial) + new_config = config_initial + + print('begin main control loop ...') + for i in np.arange(np.shape(T_se_post)[0]-1): + # write down Xd + Xd = np.array([[T_se_post[i,0],T_se_post[i,1],T_se_post[i,2],T_se_post[i,9]],[T_se_post[i,3],T_se_post[i,4],T_se_post[i,5],T_se_post[i,10]],[T_se_post[i,6],T_se_post[i,7],T_se_post[i,8],T_se_post[i,11]],[0,0,0,1]]) + + # write down Xd_next + Xd_next = np.array([[T_se_post[i+1,0],T_se_post[i+1,1],T_se_post[i+1,2],T_se_post[i+1,9]],[T_se_post[i+1,3],T_se_post[i+1,4],T_se_post[i+1,5],T_se_post[i+1,10]],[T_se_post[i+1,6],T_se_post[i+1,7],T_se_post[i+1,8],T_se_post[i+1,11]],[0,0,0,1]]) + + # compute speeds and X_err + V, speeds, X_err = feedbackControl(new_config, Xd, Xd_next, Kp, Ki, dt, joint_limit) + + # adjust the order of wheels speeds and joints speeds + speeds = np.array([speeds[4], speeds[5], speeds[6], speeds[7], speeds[8], speeds[0], speeds[1], speeds[2], speeds[3]]).reshape(1,9) + speeds_max = 1000 + + # compute new configuration + new_config = nextState(new_config,speeds,dt,speeds_max) + new_config = np.append(new_config,[[T_se_post[i,12]]],axis=1) + reference_trajectory.append(new_config) # record the trajectory + + # set commands for the youBot model in VREP + desired_world_positions[0] = new_config[0, 1] + desired_world_positions[1] = new_config[0, 2] + desired_world_positions[2] = new_config[0, 0] + + desired_arm_joint_angles[0] = new_config[0, 3] + desired_arm_joint_angles[1] = new_config[0, 4] + desired_arm_joint_angles[2] = new_config[0, 5] + desired_arm_joint_angles[3] = new_config[0, 6] + desired_arm_joint_angles[4] = new_config[0, 7] + + desired_wheel_positions[0] = new_config[0, 8] + desired_wheel_positions[1] = new_config[0, 9] + desired_wheel_positions[2] = new_config[0, 10] + desired_wheel_positions[3] = new_config[0, 11] + + # send commands to VREP + VREP_armControl(vrep_sim, clientID, arm_joints_handle, desired_arm_joint_angles) + VREP_wheelsControl(vrep_sim, clientID, wheel_joints_handle, desired_wheel_positions) + VREP_WorldFrameControl(vrep_sim, clientID, world_joints_handle, desired_world_positions) + + # set gripper state + if new_config[0, 12] == 0: + vrep_sim.simxSetJointTargetVelocity(clientID, gripper_joint_2_handle, -0.04, vrep_sim.simx_opmode_oneshot) # open + else: + vrep_sim.simxSetJointTargetVelocity(clientID, gripper_joint_2_handle, 0.04, vrep_sim.simx_opmode_oneshot) # close + return_code, gripper_joint_2_position = vrep_sim.simxGetJointPosition(clientID, gripper_joint_2_handle, vrep_sim.simx_opmode_oneshot) + vrep_sim.simxSetJointTargetPosition(clientID, gripper_joint_1_handle, -gripper_joint_2_position, vrep_sim.simx_opmode_oneshot) + time.sleep(0.002) + + # Squeeze reference trajectory + reference_trajectory = np.squeeze(reference_trajectory) + +# Stop VREP simulation +#print ('Stop VREP simulation') +#vrep_sim.simxStopSimulation(clientID, vrep_sim.simx_opmode_blocking) +time.sleep(0.1) + +# Now close the connection to VREP +vrep_sim.simxFinish(clientID) +print ('Program ended') diff --git a/7_Demo_youBotPickAndPlace/code/modern_robotics/__init__.py b/7_Demo_youBotPickAndPlace/code/modern_robotics/__init__.py new file mode 100644 index 0000000..1c4a7c8 --- /dev/null +++ b/7_Demo_youBotPickAndPlace/code/modern_robotics/__init__.py @@ -0,0 +1,4 @@ +from .__version__ import __version__ + +from .core import * + diff --git a/7_Demo_youBotPickAndPlace/code/modern_robotics/__version__.py b/7_Demo_youBotPickAndPlace/code/modern_robotics/__version__.py new file mode 100644 index 0000000..d930acc --- /dev/null +++ b/7_Demo_youBotPickAndPlace/code/modern_robotics/__version__.py @@ -0,0 +1,2 @@ + +__version__ = '1.1.0' diff --git a/7_Demo_youBotPickAndPlace/code/modern_robotics/core.py b/7_Demo_youBotPickAndPlace/code/modern_robotics/core.py new file mode 100644 index 0000000..4b44235 --- /dev/null +++ b/7_Demo_youBotPickAndPlace/code/modern_robotics/core.py @@ -0,0 +1,1899 @@ +from __future__ import print_function +''' +*************************************************************************** +Modern Robotics: Mechanics, Planning, and Control. +Code Library +*************************************************************************** +Author: Huan Weng, Bill Hunt, Jarvis Schultz, Mikhail Todes, +Email: huanweng@u.northwestern.edu +Date: January 2018 +*************************************************************************** +Language: Python +Also available in: MATLAB, Mathematica +Required library: numpy +Optional library: matplotlib +*************************************************************************** +''' + +''' +*** IMPORTS *** +''' + +import numpy as np + +''' +*** BASIC HELPER FUNCTIONS *** +''' + +def NearZero(z): + """Determines whether a scalar is small enough to be treated as zero + + :param z: A scalar input to check + :return: True if z is close to zero, false otherwise + + Example Input: + z = -1e-7 + Output: + True + """ + return abs(z) < 1e-6 + +def Normalize(V): + """Normalizes a vector + + :param V: A vector + :return: A unit vector pointing in the same direction as z + + Example Input: + V = np.array([1, 2, 3]) + Output: + np.array([0.26726124, 0.53452248, 0.80178373]) + """ + return V / np.linalg.norm(V) + +''' +*** CHAPTER 3: RIGID-BODY MOTIONS *** +''' + +def RotInv(R): + """Inverts a rotation matrix + + :param R: A rotation matrix + :return: The inverse of R + + Example Input: + R = np.array([[0, 0, 1], + [1, 0, 0], + [0, 1, 0]]) + Output: + np.array([[0, 1, 0], + [0, 0, 1], + [1, 0, 0]]) + """ + return np.array(R).T + +def VecToso3(omg): + """Converts a 3-vector to an so(3) representation + + :param omg: A 3-vector + :return: The skew symmetric representation of omg + + Example Input: + omg = np.array([1, 2, 3]) + Output: + np.array([[ 0, -3, 2], + [ 3, 0, -1], + [-2, 1, 0]]) + """ + return np.array([[0, -omg[2], omg[1]], + [omg[2], 0, -omg[0]], + [-omg[1], omg[0], 0]]) + +def so3ToVec(so3mat): + """Converts an so(3) representation to a 3-vector + + :param so3mat: A 3x3 skew-symmetric matrix + :return: The 3-vector corresponding to so3mat + + Example Input: + so3mat = np.array([[ 0, -3, 2], + [ 3, 0, -1], + [-2, 1, 0]]) + Output: + np.array([1, 2, 3]) + """ + return np.array([so3mat[2][1], so3mat[0][2], so3mat[1][0]]) + +def AxisAng3(expc3): + """Converts a 3-vector of exponential coordinates for rotation into + axis-angle form + + :param expc3: A 3-vector of exponential coordinates for rotation + :return omghat: A unit rotation axis + :return theta: The corresponding rotation angle + + Example Input: + expc3 = np.array([1, 2, 3]) + Output: + (np.array([0.26726124, 0.53452248, 0.80178373]), 3.7416573867739413) + """ + return (Normalize(expc3), np.linalg.norm(expc3)) + +def MatrixExp3(so3mat): + """Computes the matrix exponential of a matrix in so(3) + + :param so3mat: A 3x3 skew-symmetric matrix + :return: The matrix exponential of so3mat + + Example Input: + so3mat = np.array([[ 0, -3, 2], + [ 3, 0, -1], + [-2, 1, 0]]) + Output: + np.array([[-0.69492056, 0.71352099, 0.08929286], + [-0.19200697, -0.30378504, 0.93319235], + [ 0.69297817, 0.6313497 , 0.34810748]]) + """ + omgtheta = so3ToVec(so3mat) + if NearZero(np.linalg.norm(omgtheta)): + return np.eye(3) + else: + theta = AxisAng3(omgtheta)[1] + omgmat = so3mat / theta + return np.eye(3) + np.sin(theta) * omgmat \ + + (1 - np.cos(theta)) * np.dot(omgmat, omgmat) + +def MatrixLog3(R): + """Computes the matrix logarithm of a rotation matrix + + :param R: A 3x3 rotation matrix + :return: The matrix logarithm of R + + Example Input: + R = np.array([[0, 0, 1], + [1, 0, 0], + [0, 1, 0]]) + Output: + np.array([[ 0, -1.20919958, 1.20919958], + [ 1.20919958, 0, -1.20919958], + [-1.20919958, 1.20919958, 0]]) + """ + acosinput = (np.trace(R) - 1) / 2.0 + if acosinput >= 1: + return np.zeros((3, 3)) + elif acosinput <= -1: + if not NearZero(1 + R[2][2]): + omg = (1.0 / np.sqrt(2 * (1 + R[2][2]))) \ + * np.array([R[0][2], R[1][2], 1 + R[2][2]]) + elif not NearZero(1 + R[1][1]): + omg = (1.0 / np.sqrt(2 * (1 + R[1][1]))) \ + * np.array([R[0][1], 1 + R[1][1], R[2][1]]) + else: + omg = (1.0 / np.sqrt(2 * (1 + R[0][0]))) \ + * np.array([1 + R[0][0], R[1][0], R[2][0]]) + return VecToso3(np.pi * omg) + else: + theta = np.arccos(acosinput) + return theta / 2.0 / np.sin(theta) * (R - np.array(R).T) + +def RpToTrans(R, p): + """Converts a rotation matrix and a position vector into homogeneous + transformation matrix + + :param R: A 3x3 rotation matrix + :param p: A 3-vector + :return: A homogeneous transformation matrix corresponding to the inputs + + Example Input: + R = np.array([[1, 0, 0], + [0, 0, -1], + [0, 1, 0]]) + p = np.array([1, 2, 5]) + Output: + np.array([[1, 0, 0, 1], + [0, 0, -1, 2], + [0, 1, 0, 5], + [0, 0, 0, 1]]) + """ + return np.r_[np.c_[R, p], [[0, 0, 0, 1]]] + +def TransToRp(T): + """Converts a homogeneous transformation matrix into a rotation matrix + and position vector + + :param T: A homogeneous transformation matrix + :return R: The corresponding rotation matrix, + :return p: The corresponding position vector. + + Example Input: + T = np.array([[1, 0, 0, 0], + [0, 0, -1, 0], + [0, 1, 0, 3], + [0, 0, 0, 1]]) + Output: + (np.array([[1, 0, 0], + [0, 0, -1], + [0, 1, 0]]), + np.array([0, 0, 3])) + """ + T = np.array(T) + return T[0: 3, 0: 3], T[0: 3, 3] + +def TransInv(T): + """Inverts a homogeneous transformation matrix + + :param T: A homogeneous transformation matrix + :return: The inverse of T + Uses the structure of transformation matrices to avoid taking a matrix + inverse, for efficiency. + + Example input: + T = np.array([[1, 0, 0, 0], + [0, 0, -1, 0], + [0, 1, 0, 3], + [0, 0, 0, 1]]) + Output: + np.array([[1, 0, 0, 0], + [0, 0, 1, -3], + [0, -1, 0, 0], + [0, 0, 0, 1]]) + """ + R, p = TransToRp(T) + Rt = np.array(R).T + return np.r_[np.c_[Rt, -np.dot(Rt, p)], [[0, 0, 0, 1]]] + +def VecTose3(V): + """Converts a spatial velocity vector into a 4x4 matrix in se3 + + :param V: A 6-vector representing a spatial velocity + :return: The 4x4 se3 representation of V + + Example Input: + V = np.array([1, 2, 3, 4, 5, 6]) + Output: + np.array([[ 0, -3, 2, 4], + [ 3, 0, -1, 5], + [-2, 1, 0, 6], + [ 0, 0, 0, 0]]) + """ + return np.r_[np.c_[VecToso3([V[0], V[1], V[2]]), [V[3], V[4], V[5]]], + np.zeros((1, 4))] + +def se3ToVec(se3mat): + """ Converts an se3 matrix into a spatial velocity vector + + :param se3mat: A 4x4 matrix in se3 + :return: The spatial velocity 6-vector corresponding to se3mat + + Example Input: + se3mat = np.array([[ 0, -3, 2, 4], + [ 3, 0, -1, 5], + [-2, 1, 0, 6], + [ 0, 0, 0, 0]]) + Output: + np.array([1, 2, 3, 4, 5, 6]) + """ + return np.r_[[se3mat[2][1], se3mat[0][2], se3mat[1][0]], + [se3mat[0][3], se3mat[1][3], se3mat[2][3]]] + +def Adjoint(T): + """Computes the adjoint representation of a homogeneous transformation + matrix + + :param T: A homogeneous transformation matrix + :return: The 6x6 adjoint representation [AdT] of T + + Example Input: + T = np.array([[1, 0, 0, 0], + [0, 0, -1, 0], + [0, 1, 0, 3], + [0, 0, 0, 1]]) + Output: + np.array([[1, 0, 0, 0, 0, 0], + [0, 0, -1, 0, 0, 0], + [0, 1, 0, 0, 0, 0], + [0, 0, 3, 1, 0, 0], + [3, 0, 0, 0, 0, -1], + [0, 0, 0, 0, 1, 0]]) + """ + R, p = TransToRp(T) + return np.r_[np.c_[R, np.zeros((3, 3))], + np.c_[np.dot(VecToso3(p), R), R]] + +def ScrewToAxis(q, s, h): + """Takes a parametric description of a screw axis and converts it to a + normalized screw axis + + :param q: A point lying on the screw axis + :param s: A unit vector in the direction of the screw axis + :param h: The pitch of the screw axis + :return: A normalized screw axis described by the inputs + + Example Input: + q = np.array([3, 0, 0]) + s = np.array([0, 0, 1]) + h = 2 + Output: + np.array([0, 0, 1, 0, -3, 2]) + """ + return np.r_[s, np.cross(q, s) + np.dot(h, s)] + +def AxisAng6(expc6): + """Converts a 6-vector of exponential coordinates into screw axis-angle + form + + :param expc6: A 6-vector of exponential coordinates for rigid-body motion + S*theta + :return S: The corresponding normalized screw axis + :return theta: The distance traveled along/about S + + Example Input: + expc6 = np.array([1, 0, 0, 1, 2, 3]) + Output: + (np.array([1.0, 0.0, 0.0, 1.0, 2.0, 3.0]), 1.0) + """ + theta = np.linalg.norm([expc6[0], expc6[1], expc6[2]]) + if NearZero(theta): + theta = np.linalg.norm([expc6[3], expc6[4], expc6[5]]) + return (np.array(expc6 / theta), theta) + +def MatrixExp6(se3mat): + """Computes the matrix exponential of an se3 representation of + exponential coordinates + + :param se3mat: A matrix in se3 + :return: The matrix exponential of se3mat + + Example Input: + se3mat = np.array([[0, 0, 0, 0], + [0, 0, -1.57079632, 2.35619449], + [0, 1.57079632, 0, 2.35619449], + [0, 0, 0, 0]]) + Output: + np.array([[1.0, 0.0, 0.0, 0.0], + [0.0, 0.0, -1.0, 0.0], + [0.0, 1.0, 0.0, 3.0], + [ 0, 0, 0, 1]]) + """ + se3mat = np.array(se3mat) + omgtheta = so3ToVec(se3mat[0: 3, 0: 3]) + if NearZero(np.linalg.norm(omgtheta)): + return np.r_[np.c_[np.eye(3), se3mat[0: 3, 3]], [[0, 0, 0, 1]]] + else: + theta = AxisAng3(omgtheta)[1] + omgmat = se3mat[0: 3, 0: 3] / theta + return np.r_[np.c_[MatrixExp3(se3mat[0: 3, 0: 3]), + np.dot(np.eye(3) * theta \ + + (1 - np.cos(theta)) * omgmat \ + + (theta - np.sin(theta)) \ + * np.dot(omgmat,omgmat), + se3mat[0: 3, 3]) / theta], + [[0, 0, 0, 1]]] + +def MatrixLog6(T): + """Computes the matrix logarithm of a homogeneous transformation matrix + + :param R: A matrix in SE3 + :return: The matrix logarithm of R + + Example Input: + T = np.array([[1, 0, 0, 0], + [0, 0, -1, 0], + [0, 1, 0, 3], + [0, 0, 0, 1]]) + Output: + np.array([[0, 0, 0, 0] + [0, 0, -1.57079633, 2.35619449] + [0, 1.57079633, 0, 2.35619449] + [0, 0, 0, 0]]) + """ + R, p = TransToRp(T) + omgmat = MatrixLog3(R) + if np.array_equal(omgmat, np.zeros((3, 3))): + return np.r_[np.c_[np.zeros((3, 3)), + [T[0][3], T[1][3], T[2][3]]], + [[0, 0, 0, 0]]] + else: + theta = np.arccos((np.trace(R) - 1) / 2.0) + return np.r_[np.c_[omgmat, + np.dot(np.eye(3) - omgmat / 2.0 \ + + (1.0 / theta - 1.0 / np.tan(theta / 2.0) / 2) \ + * np.dot(omgmat,omgmat) / theta,[T[0][3], + T[1][3], + T[2][3]])], + [[0, 0, 0, 0]]] + +def ProjectToSO3(mat): + """Returns a projection of mat into SO(3) + + :param mat: A matrix near SO(3) to project to SO(3) + :return: The closest matrix to R that is in SO(3) + Projects a matrix mat to the closest matrix in SO(3) using singular-value + decomposition (see + http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review). + This function is only appropriate for matrices close to SO(3). + + Example Input: + mat = np.array([[ 0.675, 0.150, 0.720], + [ 0.370, 0.771, -0.511], + [-0.630, 0.619, 0.472]]) + Output: + np.array([[ 0.67901136, 0.14894516, 0.71885945], + [ 0.37320708, 0.77319584, -0.51272279], + [-0.63218672, 0.61642804, 0.46942137]]) + """ + U, s, Vh = np.linalg.svd(mat) + R = np.dot(U, Vh) + if np.linalg.det(R) < 0: + # In this case the result may be far from mat. + R[:, s[2, 2]] = -R[:, s[2, 2]] + return R + +def ProjectToSE3(mat): + """Returns a projection of mat into SE(3) + + :param mat: A 4x4 matrix to project to SE(3) + :return: The closest matrix to T that is in SE(3) + Projects a matrix mat to the closest matrix in SE(3) using singular-value + decomposition (see + http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review). + This function is only appropriate for matrices close to SE(3). + + Example Input: + mat = np.array([[ 0.675, 0.150, 0.720, 1.2], + [ 0.370, 0.771, -0.511, 5.4], + [-0.630, 0.619, 0.472, 3.6], + [ 0.003, 0.002, 0.010, 0.9]]) + Output: + np.array([[ 0.67901136, 0.14894516, 0.71885945, 1.2 ], + [ 0.37320708, 0.77319584, -0.51272279, 5.4 ], + [-0.63218672, 0.61642804, 0.46942137, 3.6 ], + [ 0. , 0. , 0. , 1. ]]) + """ + mat = np.array(mat) + return RpToTrans(ProjectToSO3(mat[:3, :3]), mat[:3, 3]) + +def DistanceToSO3(mat): + """Returns the Frobenius norm to describe the distance of mat from the + SO(3) manifold + + :param mat: A 3x3 matrix + :return: A quantity describing the distance of mat from the SO(3) + manifold + Computes the distance from mat to the SO(3) manifold using the following + method: + If det(mat) <= 0, return a large number. + If det(mat) > 0, return norm(mat^T.mat - I). + + Example Input: + mat = np.array([[ 1.0, 0.0, 0.0 ], + [ 0.0, 0.1, -0.95], + [ 0.0, 1.0, 0.1 ]]) + Output: + 0.08835 + """ + if np.linalg.det(mat) > 0: + return np.linalg.norm(np.dot(np.array(mat).T, mat) - np.eye(3)) + else: + return 1e+9 + +def DistanceToSE3(mat): + """Returns the Frobenius norm to describe the distance of mat from the + SE(3) manifold + + :param mat: A 4x4 matrix + :return: A quantity describing the distance of mat from the SE(3) + manifold + Computes the distance from mat to the SE(3) manifold using the following + method: + Compute the determinant of matR, the top 3x3 submatrix of mat. + If det(matR) <= 0, return a large number. + If det(matR) > 0, replace the top 3x3 submatrix of mat with matR^T.matR, + and set the first three entries of the fourth column of mat to zero. Then + return norm(mat - I). + + Example Input: + mat = np.array([[ 1.0, 0.0, 0.0, 1.2 ], + [ 0.0, 0.1, -0.95, 1.5 ], + [ 0.0, 1.0, 0.1, -0.9 ], + [ 0.0, 0.0, 0.1, 0.98 ]]) + Output: + 0.134931 + """ + matR = np.array(mat)[0: 3, 0: 3] + if np.linalg.det(matR) > 0: + return np.linalg.norm(np.r_[np.c_[np.dot(np.transpose(matR), matR), + np.zeros((3, 1))], + [np.array(mat)[3, :]]] - np.eye(4)) + else: + return 1e+9 + +def TestIfSO3(mat): + """Returns true if mat is close to or on the manifold SO(3) + + :param mat: A 3x3 matrix + :return: True if mat is very close to or in SO(3), false otherwise + Computes the distance d from mat to the SO(3) manifold using the + following method: + If det(mat) <= 0, d = a large number. + If det(mat) > 0, d = norm(mat^T.mat - I). + If d is close to zero, return true. Otherwise, return false. + + Example Input: + mat = np.array([[1.0, 0.0, 0.0 ], + [0.0, 0.1, -0.95], + [0.0, 1.0, 0.1 ]]) + Output: + False + """ + return abs(DistanceToSO3(mat)) < 1e-3 + +def TestIfSE3(mat): + """Returns true if mat is close to or on the manifold SE(3) + + :param mat: A 4x4 matrix + :return: True if mat is very close to or in SE(3), false otherwise + Computes the distance d from mat to the SE(3) manifold using the + following method: + Compute the determinant of the top 3x3 submatrix of mat. + If det(mat) <= 0, d = a large number. + If det(mat) > 0, replace the top 3x3 submatrix of mat with mat^T.mat, and + set the first three entries of the fourth column of mat to zero. + Then d = norm(T - I). + If d is close to zero, return true. Otherwise, return false. + + Example Input: + mat = np.array([[1.0, 0.0, 0.0, 1.2], + [0.0, 0.1, -0.95, 1.5], + [0.0, 1.0, 0.1, -0.9], + [0.0, 0.0, 0.1, 0.98]]) + Output: + False + """ + return abs(DistanceToSE3(mat)) < 1e-3 + +''' +*** CHAPTER 4: FORWARD KINEMATICS *** +''' + +def FKinBody(M, Blist, thetalist): + """Computes forward kinematics in the body frame for an open chain robot + + :param M: The home configuration (position and orientation) of the end- + effector + :param Blist: The joint screw axes in the end-effector frame when the + manipulator is at the home position, in the format of a + matrix with axes as the columns + :param thetalist: A list of joint coordinates + :return: A homogeneous transformation matrix representing the end- + effector frame when the joints are at the specified coordinates + (i.t.o Body Frame) + + Example Input: + M = np.array([[-1, 0, 0, 0], + [ 0, 1, 0, 6], + [ 0, 0, -1, 2], + [ 0, 0, 0, 1]]) + Blist = np.array([[0, 0, -1, 2, 0, 0], + [0, 0, 0, 0, 1, 0], + [0, 0, 1, 0, 0, 0.1]]).T + thetalist = np.array([np.pi / 2.0, 3, np.pi]) + Output: + np.array([[0, 1, 0, -5], + [1, 0, 0, 4], + [0, 0, -1, 1.68584073], + [0, 0, 0, 1]]) + """ + T = np.array(M) + for i in range(len(thetalist)): + T = np.dot(T, MatrixExp6(VecTose3(np.array(Blist)[:, i] \ + * thetalist[i]))) + return T + +def FKinSpace(M, Slist, thetalist): + """Computes forward kinematics in the space frame for an open chain robot + + :param M: The home configuration (position and orientation) of the end- + effector + :param Slist: The joint screw axes in the space frame when the + manipulator is at the home position, in the format of a + matrix with axes as the columns + :param thetalist: A list of joint coordinates + :return: A homogeneous transformation matrix representing the end- + effector frame when the joints are at the specified coordinates + (i.t.o Space Frame) + + Example Input: + M = np.array([[-1, 0, 0, 0], + [ 0, 1, 0, 6], + [ 0, 0, -1, 2], + [ 0, 0, 0, 1]]) + Slist = np.array([[0, 0, 1, 4, 0, 0], + [0, 0, 0, 0, 1, 0], + [0, 0, -1, -6, 0, -0.1]]).T + thetalist = np.array([np.pi / 2.0, 3, np.pi]) + Output: + np.array([[0, 1, 0, -5], + [1, 0, 0, 4], + [0, 0, -1, 1.68584073], + [0, 0, 0, 1]]) + """ + T = np.array(M) + for i in range(len(thetalist) - 1, -1, -1): + T = np.dot(MatrixExp6(VecTose3(np.array(Slist)[:, i] \ + * thetalist[i])), T) + return T + +''' +*** CHAPTER 5: VELOCITY KINEMATICS AND STATICS*** +''' + +def JacobianBody(Blist, thetalist): + """Computes the body Jacobian for an open chain robot + + :param Blist: The joint screw axes in the end-effector frame when the + manipulator is at the home position, in the format of a + matrix with axes as the columns + :param thetalist: A list of joint coordinates + :return: The body Jacobian corresponding to the inputs (6xn real + numbers) + + Example Input: + Blist = np.array([[0, 0, 1, 0, 0.2, 0.2], + [1, 0, 0, 2, 0, 3], + [0, 1, 0, 0, 2, 1], + [1, 0, 0, 0.2, 0.3, 0.4]]).T + thetalist = np.array([0.2, 1.1, 0.1, 1.2]) + Output: + np.array([[-0.04528405, 0.99500417, 0, 1] + [ 0.74359313, 0.09304865, 0.36235775, 0] + [-0.66709716, 0.03617541, -0.93203909, 0] + [ 2.32586047, 1.66809, 0.56410831, 0.2] + [-1.44321167, 2.94561275, 1.43306521, 0.3] + [-2.06639565, 1.82881722, -1.58868628, 0.4]]) + """ + Jb = np.array(Blist).copy().astype(np.float) + T = np.eye(4) + for i in range(len(thetalist) - 2, -1, -1): + T = np.dot(T,MatrixExp6(VecTose3(np.array(Blist)[:, i + 1] \ + * -thetalist[i + 1]))) + Jb[:, i] = np.dot(Adjoint(T), np.array(Blist)[:, i]) + return Jb + +def JacobianSpace(Slist, thetalist): + """Computes the space Jacobian for an open chain robot + + :param Slist: The joint screw axes in the space frame when the + manipulator is at the home position, in the format of a + matrix with axes as the columns + :param thetalist: A list of joint coordinates + :return: The space Jacobian corresponding to the inputs (6xn real + numbers) + + Example Input: + Slist = np.array([[0, 0, 1, 0, 0.2, 0.2], + [1, 0, 0, 2, 0, 3], + [0, 1, 0, 0, 2, 1], + [1, 0, 0, 0.2, 0.3, 0.4]]).T + thetalist = np.array([0.2, 1.1, 0.1, 1.2]) + Output: + np.array([[ 0, 0.98006658, -0.09011564, 0.95749426] + [ 0, 0.19866933, 0.4445544, 0.28487557] + [ 1, 0, 0.89120736, -0.04528405] + [ 0, 1.95218638, -2.21635216, -0.51161537] + [0.2, 0.43654132, -2.43712573, 2.77535713] + [0.2, 2.96026613, 3.23573065, 2.22512443]]) + """ + Js = np.array(Slist).copy().astype(np.float) + T = np.eye(4) + for i in range(1, len(thetalist)): + T = np.dot(T, MatrixExp6(VecTose3(np.array(Slist)[:, i - 1] \ + * thetalist[i - 1]))) + Js[:, i] = np.dot(Adjoint(T), np.array(Slist)[:, i]) + return Js + +''' +*** CHAPTER 6: INVERSE KINEMATICS *** +''' + +def IKinBody(Blist, M, T, thetalist0, eomg, ev): + """Computes inverse kinematics in the body frame for an open chain robot + + :param Blist: The joint screw axes in the end-effector frame when the + manipulator is at the home position, in the format of a + matrix with axes as the columns + :param M: The home configuration of the end-effector + :param T: The desired end-effector configuration Tsd + :param thetalist0: An initial guess of joint angles that are close to + satisfying Tsd + :param eomg: A small positive tolerance on the end-effector orientation + error. The returned joint angles must give an end-effector + orientation error less than eomg + :param ev: A small positive tolerance on the end-effector linear position + error. The returned joint angles must give an end-effector + position error less than ev + :return thetalist: Joint angles that achieve T within the specified + tolerances, + :return success: A logical value where TRUE means that the function found + a solution and FALSE means that it ran through the set + number of maximum iterations without finding a solution + within the tolerances eomg and ev. + Uses an iterative Newton-Raphson root-finding method. + The maximum number of iterations before the algorithm is terminated has + been hardcoded in as a variable called maxiterations. It is set to 20 at + the start of the function, but can be changed if needed. + + Example Input: + Blist = np.array([[0, 0, -1, 2, 0, 0], + [0, 0, 0, 0, 1, 0], + [0, 0, 1, 0, 0, 0.1]]).T + M = np.array([[-1, 0, 0, 0], + [ 0, 1, 0, 6], + [ 0, 0, -1, 2], + [ 0, 0, 0, 1]]) + T = np.array([[0, 1, 0, -5], + [1, 0, 0, 4], + [0, 0, -1, 1.6858], + [0, 0, 0, 1]]) + thetalist0 = np.array([1.5, 2.5, 3]) + eomg = 0.01 + ev = 0.001 + Output: + (np.array([1.57073819, 2.999667, 3.14153913]), True) + """ + thetalist = np.array(thetalist0).copy() + i = 0 + maxiterations = 20 + Vb = se3ToVec(MatrixLog6(np.dot(TransInv(FKinBody(M, Blist, \ + thetalist)), T))) + err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg \ + or np.linalg.norm([Vb[3], Vb[4], Vb[5]]) > ev + while err and i < maxiterations: + thetalist = thetalist \ + + np.dot(np.linalg.pinv(JacobianBody(Blist, \ + thetalist)), Vb) + i = i + 1 + Vb \ + = se3ToVec(MatrixLog6(np.dot(TransInv(FKinBody(M, Blist, \ + thetalist)), T))) + err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg \ + or np.linalg.norm([Vb[3], Vb[4], Vb[5]]) > ev + return (thetalist, not err) + +def IKinSpace(Slist, M, T, thetalist0, eomg, ev): + """Computes inverse kinematics in the space frame for an open chain robot + + :param Slist: The joint screw axes in the space frame when the + manipulator is at the home position, in the format of a + matrix with axes as the columns + :param M: The home configuration of the end-effector + :param T: The desired end-effector configuration Tsd + :param thetalist0: An initial guess of joint angles that are close to + satisfying Tsd + :param eomg: A small positive tolerance on the end-effector orientation + error. The returned joint angles must give an end-effector + orientation error less than eomg + :param ev: A small positive tolerance on the end-effector linear position + error. The returned joint angles must give an end-effector + position error less than ev + :return thetalist: Joint angles that achieve T within the specified + tolerances, + :return success: A logical value where TRUE means that the function found + a solution and FALSE means that it ran through the set + number of maximum iterations without finding a solution + within the tolerances eomg and ev. + Uses an iterative Newton-Raphson root-finding method. + The maximum number of iterations before the algorithm is terminated has + been hardcoded in as a variable called maxiterations. It is set to 20 at + the start of the function, but can be changed if needed. + + Example Input: + Slist = np.array([[0, 0, 1, 4, 0, 0], + [0, 0, 0, 0, 1, 0], + [0, 0, -1, -6, 0, -0.1]]).T + M = np.array([[-1, 0, 0, 0], + [ 0, 1, 0, 6], + [ 0, 0, -1, 2], + [ 0, 0, 0, 1]]) + T = np.array([[0, 1, 0, -5], + [1, 0, 0, 4], + [0, 0, -1, 1.6858], + [0, 0, 0, 1]]) + thetalist0 = np.array([1.5, 2.5, 3]) + eomg = 0.01 + ev = 0.001 + Output: + (np.array([ 1.57073783, 2.99966384, 3.1415342 ]), True) + """ + thetalist = np.array(thetalist0).copy() + i = 0 + maxiterations = 20 + Tsb = FKinSpace(M,Slist, thetalist) + Vs = np.dot(Adjoint(Tsb), \ + se3ToVec(MatrixLog6(np.dot(TransInv(Tsb), T)))) + err = np.linalg.norm([Vs[0], Vs[1], Vs[2]]) > eomg \ + or np.linalg.norm([Vs[3], Vs[4], Vs[5]]) > ev + while err and i < maxiterations: + thetalist = thetalist \ + + np.dot(np.linalg.pinv(JacobianSpace(Slist, \ + thetalist)), Vs) + i = i + 1 + Tsb = FKinSpace(M, Slist, thetalist) + Vs = np.dot(Adjoint(Tsb), \ + se3ToVec(MatrixLog6(np.dot(TransInv(Tsb), T)))) + err = np.linalg.norm([Vs[0], Vs[1], Vs[2]]) > eomg \ + or np.linalg.norm([Vs[3], Vs[4], Vs[5]]) > ev + return (thetalist, not err) + +''' +*** CHAPTER 8: DYNAMICS OF OPEN CHAINS *** +''' + +def ad(V): + """Calculate the 6x6 matrix [adV] of the given 6-vector + + :param V: A 6-vector spatial velocity + :return: The corresponding 6x6 matrix [adV] + + Used to calculate the Lie bracket [V1, V2] = [adV1]V2 + + Example Input: + V = np.array([1, 2, 3, 4, 5, 6]) + Output: + np.array([[ 0, -3, 2, 0, 0, 0], + [ 3, 0, -1, 0, 0, 0], + [-2, 1, 0, 0, 0, 0], + [ 0, -6, 5, 0, -3, 2], + [ 6, 0, -4, 3, 0, -1], + [-5, 4, 0, -2, 1, 0]]) + """ + omgmat = VecToso3([V[0], V[1], V[2]]) + return np.r_[np.c_[omgmat, np.zeros((3, 3))], + np.c_[VecToso3([V[3], V[4], V[5]]), omgmat]] + +def InverseDynamics(thetalist, dthetalist, ddthetalist, g, Ftip, Mlist, \ + Glist, Slist): + """Computes inverse dynamics in the space frame for an open chain robot + + :param thetalist: n-vector of joint variables + :param dthetalist: n-vector of joint rates + :param ddthetalist: n-vector of joint accelerations + :param g: Gravity vector g + :param Ftip: Spatial force applied by the end-effector expressed in frame + {n+1} + :param Mlist: List of link frames {i} relative to {i-1} at the home + position + :param Glist: Spatial inertia matrices Gi of the links + :param Slist: Screw axes Si of the joints in a space frame, in the format + of a matrix with axes as the columns + :return: The n-vector of required joint forces/torques + This function uses forward-backward Newton-Euler iterations to solve the + equation: + taulist = Mlist(thetalist)ddthetalist + c(thetalist,dthetalist) \ + + g(thetalist) + Jtr(thetalist)Ftip + + Example Input (3 Link Robot): + thetalist = np.array([0.1, 0.1, 0.1]) + dthetalist = np.array([0.1, 0.2, 0.3]) + ddthetalist = np.array([2, 1.5, 1]) + g = np.array([0, 0, -9.8]) + Ftip = np.array([1, 1, 1, 1, 1, 1]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + Output: + np.array([74.69616155, -33.06766016, -3.23057314]) + """ + n = len(thetalist) + Mi = np.eye(4) + Ai = np.zeros((6, n)) + AdTi = [[None]] * (n + 1) + Vi = np.zeros((6, n + 1)) + Vdi = np.zeros((6, n + 1)) + Vdi[:, 0] = np.r_[[0, 0, 0], -np.array(g)] + AdTi[n] = Adjoint(TransInv(Mlist[n])) + Fi = np.array(Ftip).copy() + taulist = np.zeros(n) + for i in range(n): + Mi = np.dot(Mi,Mlist[i]) + Ai[:, i] = np.dot(Adjoint(TransInv(Mi)), np.array(Slist)[:, i]) + AdTi[i] = Adjoint(np.dot(MatrixExp6(VecTose3(Ai[:, i] * \ + -thetalist[i])), \ + TransInv(Mlist[i]))) + Vi[:, i + 1] = np.dot(AdTi[i], Vi[:,i]) + Ai[:, i] * dthetalist[i] + Vdi[:, i + 1] = np.dot(AdTi[i], Vdi[:, i]) \ + + Ai[:, i] * ddthetalist[i] \ + + np.dot(ad(Vi[:, i + 1]), Ai[:, i]) * dthetalist[i] + for i in range (n - 1, -1, -1): + Fi = np.dot(np.array(AdTi[i + 1]).T, Fi) \ + + np.dot(np.array(Glist[i]), Vdi[:, i + 1]) \ + - np.dot(np.array(ad(Vi[:, i + 1])).T, \ + np.dot(np.array(Glist[i]), Vi[:, i + 1])) + taulist[i] = np.dot(np.array(Fi).T, Ai[:, i]) + return taulist + +def MassMatrix(thetalist, Mlist, Glist, Slist): + """Computes the mass matrix of an open chain robot based on the given + configuration + + :param thetalist: A list of joint variables + :param Mlist: List of link frames i relative to i-1 at the home position + :param Glist: Spatial inertia matrices Gi of the links + :param Slist: Screw axes Si of the joints in a space frame, in the format + of a matrix with axes as the columns + :return: The numerical inertia matrix M(thetalist) of an n-joint serial + chain at the given configuration thetalist + This function calls InverseDynamics n times, each time passing a + ddthetalist vector with a single element equal to one and all other + inputs set to zero. + Each call of InverseDynamics generates a single column, and these columns + are assembled to create the inertia matrix. + + Example Input (3 Link Robot): + thetalist = np.array([0.1, 0.1, 0.1]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + Output: + np.array([[ 2.25433380e+01, -3.07146754e-01, -7.18426391e-03] + [-3.07146754e-01, 1.96850717e+00, 4.32157368e-01] + [-7.18426391e-03, 4.32157368e-01, 1.91630858e-01]]) + """ + n = len(thetalist) + M = np.zeros((n, n)) + for i in range (n): + ddthetalist = [0] * n + ddthetalist[i] = 1 + M[:, i] = InverseDynamics(thetalist, [0] * n, ddthetalist, \ + [0, 0, 0], [0, 0, 0, 0, 0, 0], Mlist, \ + Glist, Slist) + return M + +def VelQuadraticForces(thetalist, dthetalist, Mlist, Glist, Slist): + """Computes the Coriolis and centripetal terms in the inverse dynamics of + an open chain robot + + :param thetalist: A list of joint variables, + :param dthetalist: A list of joint rates, + :param Mlist: List of link frames i relative to i-1 at the home position, + :param Glist: Spatial inertia matrices Gi of the links, + :param Slist: Screw axes Si of the joints in a space frame, in the format + of a matrix with axes as the columns. + :return: The vector c(thetalist,dthetalist) of Coriolis and centripetal + terms for a given thetalist and dthetalist. + This function calls InverseDynamics with g = 0, Ftip = 0, and + ddthetalist = 0. + + Example Input (3 Link Robot): + thetalist = np.array([0.1, 0.1, 0.1]) + dthetalist = np.array([0.1, 0.2, 0.3]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + Output: + np.array([0.26453118, -0.05505157, -0.00689132]) + """ + return InverseDynamics(thetalist, dthetalist, [0] * len(thetalist), \ + [0, 0, 0], [0, 0, 0, 0, 0, 0], Mlist, Glist, \ + Slist) + +def GravityForces(thetalist, g, Mlist, Glist, Slist): + """Computes the joint forces/torques an open chain robot requires to + overcome gravity at its configuration + + :param thetalist: A list of joint variables + :param g: 3-vector for gravitational acceleration + :param Mlist: List of link frames i relative to i-1 at the home position + :param Glist: Spatial inertia matrices Gi of the links + :param Slist: Screw axes Si of the joints in a space frame, in the format + of a matrix with axes as the columns + :return grav: The joint forces/torques required to overcome gravity at + thetalist + This function calls InverseDynamics with Ftip = 0, dthetalist = 0, and + ddthetalist = 0. + + Example Inputs (3 Link Robot): + thetalist = np.array([0.1, 0.1, 0.1]) + g = np.array([0, 0, -9.8]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + Output: + np.array([28.40331262, -37.64094817, -5.4415892]) + """ + n = len(thetalist) + return InverseDynamics(thetalist, [0] * n, [0] * n, g, \ + [0, 0, 0, 0, 0, 0], Mlist, Glist, Slist) + +def EndEffectorForces(thetalist, Ftip, Mlist, Glist, Slist): + """Computes the joint forces/torques an open chain robot requires only to + create the end-effector force Ftip + + :param thetalist: A list of joint variables + :param Ftip: Spatial force applied by the end-effector expressed in frame + {n+1} + :param Mlist: List of link frames i relative to i-1 at the home position + :param Glist: Spatial inertia matrices Gi of the links + :param Slist: Screw axes Si of the joints in a space frame, in the format + of a matrix with axes as the columns + :return: The joint forces and torques required only to create the + end-effector force Ftip + This function calls InverseDynamics with g = 0, dthetalist = 0, and + ddthetalist = 0. + + Example Input (3 Link Robot): + thetalist = np.array([0.1, 0.1, 0.1]) + Ftip = np.array([1, 1, 1, 1, 1, 1]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + Output: + np.array([1.40954608, 1.85771497, 1.392409]) + """ + n = len(thetalist) + return InverseDynamics(thetalist, [0] * n, [0] * n, [0, 0, 0], Ftip, \ + Mlist, Glist, Slist) + +def ForwardDynamics(thetalist, dthetalist, taulist, g, Ftip, Mlist, \ + Glist, Slist): + """Computes forward dynamics in the space frame for an open chain robot + + :param thetalist: A list of joint variables + :param dthetalist: A list of joint rates + :param taulist: An n-vector of joint forces/torques + :param g: Gravity vector g + :param Ftip: Spatial force applied by the end-effector expressed in frame + {n+1} + :param Mlist: List of link frames i relative to i-1 at the home position + :param Glist: Spatial inertia matrices Gi of the links + :param Slist: Screw axes Si of the joints in a space frame, in the format + of a matrix with axes as the columns + :return: The resulting joint accelerations + This function computes ddthetalist by solving: + Mlist(thetalist) * ddthetalist = taulist - c(thetalist,dthetalist) \ + - g(thetalist) - Jtr(thetalist) * Ftip + + Example Input (3 Link Robot): + thetalist = np.array([0.1, 0.1, 0.1]) + dthetalist = np.array([0.1, 0.2, 0.3]) + taulist = np.array([0.5, 0.6, 0.7]) + g = np.array([0, 0, -9.8]) + Ftip = np.array([1, 1, 1, 1, 1, 1]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + Output: + np.array([-0.97392907, 25.58466784, -32.91499212]) + """ + return np.dot(np.linalg.inv(MassMatrix(thetalist, Mlist, Glist, \ + Slist)), \ + np.array(taulist) \ + - VelQuadraticForces(thetalist, dthetalist, Mlist, \ + Glist, Slist) \ + - GravityForces(thetalist, g, Mlist, Glist, Slist) \ + - EndEffectorForces(thetalist, Ftip, Mlist, Glist, \ + Slist)) + +def EulerStep(thetalist, dthetalist, ddthetalist, dt): + """Compute the joint angles and velocities at the next timestep using from here + first order Euler integration + + :param thetalist: n-vector of joint variables + :param dthetalist: n-vector of joint rates + :param ddthetalist: n-vector of joint accelerations + :param dt: The timestep delta t + :return thetalistNext: Vector of joint variables after dt from first + order Euler integration + :return dthetalistNext: Vector of joint rates after dt from first order + Euler integration + + Example Inputs (3 Link Robot): + thetalist = np.array([0.1, 0.1, 0.1]) + dthetalist = np.array([0.1, 0.2, 0.3]) + ddthetalist = np.array([2, 1.5, 1]) + dt = 0.1 + Output: + thetalistNext: + array([ 0.11, 0.12, 0.13]) + dthetalistNext: + array([ 0.3 , 0.35, 0.4 ]) + """ + return thetalist + dt * np.array(dthetalist), \ + dthetalist + dt * np.array(ddthetalist) + +def InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, \ + Ftipmat, Mlist, Glist, Slist): + """Calculates the joint forces/torques required to move the serial chain + along the given trajectory using inverse dynamics + + :param thetamat: An N x n matrix of robot joint variables + :param dthetamat: An N x n matrix of robot joint velocities + :param ddthetamat: An N x n matrix of robot joint accelerations + :param g: Gravity vector g + :param Ftipmat: An N x 6 matrix of spatial forces applied by the end- + effector (If there are no tip forces the user should + input a zero and a zero matrix will be used) + :param Mlist: List of link frames i relative to i-1 at the home position + :param Glist: Spatial inertia matrices Gi of the links + :param Slist: Screw axes Si of the joints in a space frame, in the format + of a matrix with axes as the columns + :return: The N x n matrix of joint forces/torques for the specified + trajectory, where each of the N rows is the vector of joint + forces/torques at each time step + + Example Inputs (3 Link Robot): + from __future__ import print_function + import numpy as np + import modern_robotics as mr + # Create a trajectory to follow using functions from Chapter 9 + thetastart = np.array([0, 0, 0]) + thetaend = np.array([np.pi / 2, np.pi / 2, np.pi / 2]) + Tf = 3 + N= 1000 + method = 5 + traj = mr.JointTrajectory(thetastart, thetaend, Tf, N, method) + thetamat = np.array(traj).copy() + dthetamat = np.zeros((1000,3 )) + ddthetamat = np.zeros((1000, 3)) + dt = Tf / (N - 1.0) + for i in range(np.array(traj).shape[0] - 1): + dthetamat[i + 1, :] = (thetamat[i + 1, :] - thetamat[i, :]) / dt + ddthetamat[i + 1, :] \ + = (dthetamat[i + 1, :] - dthetamat[i, :]) / dt + # Initialize robot description (Example with 3 links) + g = np.array([0, 0, -9.8]) + Ftipmat = np.ones((N, 6)) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + taumat \ + = mr.InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, \ + Ftipmat, Mlist, Glist, Slist) + # Output using matplotlib to plot the joint forces/torques + Tau1 = taumat[:, 0] + Tau2 = taumat[:, 1] + Tau3 = taumat[:, 2] + timestamp = np.linspace(0, Tf, N) + try: + import matplotlib.pyplot as plt + except: + print('The result will not be plotted due to a lack of package matplotlib') + else: + plt.plot(timestamp, Tau1, label = "Tau1") + plt.plot(timestamp, Tau2, label = "Tau2") + plt.plot(timestamp, Tau3, label = "Tau3") + plt.ylim (-40, 120) + plt.legend(loc = 'lower right') + plt.xlabel("Time") + plt.ylabel("Torque") + plt.title("Plot of Torque Trajectories") + plt.show() + """ + thetamat = np.array(thetamat).T + dthetamat = np.array(dthetamat).T + ddthetamat = np.array(ddthetamat).T + Ftipmat = np.array(Ftipmat).T + taumat = np.array(thetamat).copy() + for i in range(np.array(thetamat).shape[1]): + taumat[:, i] \ + = InverseDynamics(thetamat[:, i], dthetamat[:, i], \ + ddthetamat[:, i], g, Ftipmat[:, i], Mlist, \ + Glist, Slist) + taumat = np.array(taumat).T + return taumat + +def ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, Ftipmat, \ + Mlist, Glist, Slist, dt, intRes): + """Simulates the motion of a serial chain given an open-loop history of + joint forces/torques + + :param thetalist: n-vector of initial joint variables + :param dthetalist: n-vector of initial joint rates + :param taumat: An N x n matrix of joint forces/torques, where each row is + the joint effort at any time step + :param g: Gravity vector g + :param Ftipmat: An N x 6 matrix of spatial forces applied by the end- + effector (If there are no tip forces the user should + input a zero and a zero matrix will be used) + :param Mlist: List of link frames {i} relative to {i-1} at the home + position + :param Glist: Spatial inertia matrices Gi of the links + :param Slist: Screw axes Si of the joints in a space frame, in the format + of a matrix with axes as the columns + :param dt: The timestep between consecutive joint forces/torques + :param intRes: Integration resolution is the number of times integration + (Euler) takes places between each time step. Must be an + integer value greater than or equal to 1 + :return thetamat: The N x n matrix of robot joint angles resulting from + the specified joint forces/torques + :return dthetamat: The N x n matrix of robot joint velocities + This function calls a numerical integration procedure that uses + ForwardDynamics. + + Example Inputs (3 Link Robot): + from __future__ import print_function + import numpy as np + import modern_robotics as mr + thetalist = np.array([0.1, 0.1, 0.1]) + dthetalist = np.array([0.1, 0.2, 0.3]) + taumat = np.array([[3.63, -6.58, -5.57], [3.74, -5.55, -5.5], + [4.31, -0.68, -5.19], [5.18, 5.63, -4.31], + [5.85, 8.17, -2.59], [5.78, 2.79, -1.7], + [4.99, -5.3, -1.19], [4.08, -9.41, 0.07], + [3.56, -10.1, 0.97], [3.49, -9.41, 1.23]]) + # Initialize robot description (Example with 3 links) + g = np.array([0, 0, -9.8]) + Ftipmat = np.ones((np.array(taumat).shape[0], 6)) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + dt = 0.1 + intRes = 8 + thetamat,dthetamat \ + = mr.ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, \ + Ftipmat, Mlist, Glist, Slist, dt, \ + intRes) + # Output using matplotlib to plot the joint angle/velocities + theta1 = thetamat[:, 0] + theta2 = thetamat[:, 1] + theta3 = thetamat[:, 2] + dtheta1 = dthetamat[:, 0] + dtheta2 = dthetamat[:, 1] + dtheta3 = dthetamat[:, 2] + N = np.array(taumat).shape[0] + Tf = np.array(taumat).shape[0] * dt + timestamp = np.linspace(0, Tf, N) + try: + import matplotlib.pyplot as plt + except: + print('The result will not be plotted due to a lack of package matplotlib') + else: + plt.plot(timestamp, theta1, label = "Theta1") + plt.plot(timestamp, theta2, label = "Theta2") + plt.plot(timestamp, theta3, label = "Theta3") + plt.plot(timestamp, dtheta1, label = "DTheta1") + plt.plot(timestamp, dtheta2, label = "DTheta2") + plt.plot(timestamp, dtheta3, label = "DTheta3") + plt.ylim (-12, 10) + plt.legend(loc = 'lower right') + plt.xlabel("Time") + plt.ylabel("Joint Angles/Velocities") + plt.title("Plot of Joint Angles and Joint Velocities") + plt.show() + """ + taumat = np.array(taumat).T + Ftipmat = np.array(Ftipmat).T + thetamat = taumat.copy().astype(np.float) + thetamat[:, 0] = thetalist + dthetamat = taumat.copy().astype(np.float) + dthetamat[:, 0] = dthetalist + for i in range(np.array(taumat).shape[1] - 1): + for j in range(intRes): + ddthetalist \ + = ForwardDynamics(thetalist, dthetalist, taumat[:, i], g, \ + Ftipmat[:, i], Mlist, Glist, Slist) + thetalist,dthetalist = EulerStep(thetalist, dthetalist, \ + ddthetalist, 1.0 * dt / intRes) + thetamat[:, i + 1] = thetalist + dthetamat[:, i + 1] = dthetalist + thetamat = np.array(thetamat).T + dthetamat = np.array(dthetamat).T + return thetamat, dthetamat + +''' +*** CHAPTER 9: TRAJECTORY GENERATION *** +''' + +def CubicTimeScaling(Tf, t): + """Computes s(t) for a cubic time scaling + + :param Tf: Total time of the motion in seconds from rest to rest + :param t: The current time t satisfying 0 < t < Tf + :return: The path parameter s(t) corresponding to a third-order + polynomial motion that begins and ends at zero velocity + + Example Input: + Tf = 2 + t = 0.6 + Output: + 0.216 + """ + return 3 * (1.0 * t / Tf) ** 2 - 2 * (1.0 * t / Tf) ** 3 + +def QuinticTimeScaling(Tf, t): + """Computes s(t) for a quintic time scaling + + :param Tf: Total time of the motion in seconds from rest to rest + :param t: The current time t satisfying 0 < t < Tf + :return: The path parameter s(t) corresponding to a fifth-order + polynomial motion that begins and ends at zero velocity and zero + acceleration + + Example Input: + Tf = 2 + t = 0.6 + Output: + 0.16308 + """ + return 10 * (1.0 * t / Tf) ** 3 - 15 * (1.0 * t / Tf) ** 4 \ + + 6 * (1.0 * t / Tf) ** 5 + +def JointTrajectory(thetastart, thetaend, Tf, N, method): + """Computes a straight-line trajectory in joint space + + :param thetastart: The initial joint variables + :param thetaend: The final joint variables + :param Tf: Total time of the motion in seconds from rest to rest + :param N: The number of points N > 1 (Start and stop) in the discrete + representation of the trajectory + :param method: The time-scaling method, where 3 indicates cubic (third- + order polynomial) time scaling and 5 indicates quintic + (fifth-order polynomial) time scaling + :return: A trajectory as an N x n matrix, where each row is an n-vector + of joint variables at an instant in time. The first row is + thetastart and the Nth row is thetaend . The elapsed time + between each row is Tf / (N - 1) + + Example Input: + thetastart = np.array([1, 0, 0, 1, 1, 0.2, 0,1]) + thetaend = np.array([1.2, 0.5, 0.6, 1.1, 2, 2, 0.9, 1]) + Tf = 4 + N = 6 + method = 3 + Output: + np.array([[ 1, 0, 0, 1, 1, 0.2, 0, 1] + [1.0208, 0.052, 0.0624, 1.0104, 1.104, 0.3872, 0.0936, 1] + [1.0704, 0.176, 0.2112, 1.0352, 1.352, 0.8336, 0.3168, 1] + [1.1296, 0.324, 0.3888, 1.0648, 1.648, 1.3664, 0.5832, 1] + [1.1792, 0.448, 0.5376, 1.0896, 1.896, 1.8128, 0.8064, 1] + [ 1.2, 0.5, 0.6, 1.1, 2, 2, 0.9, 1]]) + """ + N = int(N) + timegap = Tf / (N - 1.0) + traj = np.zeros((len(thetastart), N)) + for i in range(N): + if method == 3: + s = CubicTimeScaling(Tf, timegap * i) + else: + s = QuinticTimeScaling(Tf, timegap * i) + traj[:, i] = s * np.array(thetaend) + (1 - s) * np.array(thetastart) + traj = np.array(traj).T + return traj + +def ScrewTrajectory(Xstart, Xend, Tf, N, method): + """Computes a trajectory as a list of N SE(3) matrices corresponding to + the screw motion about a space screw axis + + :param Xstart: The initial end-effector configuration + :param Xend: The final end-effector configuration + :param Tf: Total time of the motion in seconds from rest to rest + :param N: The number of points N > 1 (Start and stop) in the discrete + representation of the trajectory + :param method: The time-scaling method, where 3 indicates cubic (third- + order polynomial) time scaling and 5 indicates quintic + (fifth-order polynomial) time scaling + :return: The discretized trajectory as a list of N matrices in SE(3) + separated in time by Tf/(N-1). The first in the list is Xstart + and the Nth is Xend + + Example Input: + Xstart = np.array([[1, 0, 0, 1], + [0, 1, 0, 0], + [0, 0, 1, 1], + [0, 0, 0, 1]]) + Xend = np.array([[0, 0, 1, 0.1], + [1, 0, 0, 0], + [0, 1, 0, 4.1], + [0, 0, 0, 1]]) + Tf = 5 + N = 4 + method = 3 + Output: + [np.array([[1, 0, 0, 1] + [0, 1, 0, 0] + [0, 0, 1, 1] + [0, 0, 0, 1]]), + np.array([[0.904, -0.25, 0.346, 0.441] + [0.346, 0.904, -0.25, 0.529] + [-0.25, 0.346, 0.904, 1.601] + [ 0, 0, 0, 1]]), + np.array([[0.346, -0.25, 0.904, -0.117] + [0.904, 0.346, -0.25, 0.473] + [-0.25, 0.904, 0.346, 3.274] + [ 0, 0, 0, 1]]), + np.array([[0, 0, 1, 0.1] + [1, 0, 0, 0] + [0, 1, 0, 4.1] + [0, 0, 0, 1]])] + """ + N = int(N) + timegap = Tf / (N - 1.0) + traj = [[None]] * N + for i in range(N): + if method == 3: + s = CubicTimeScaling(Tf, timegap * i) + else: + s = QuinticTimeScaling(Tf, timegap * i) + traj[i] \ + = np.dot(Xstart, MatrixExp6(MatrixLog6(np.dot(TransInv(Xstart), \ + Xend)) * s)) + return traj + +def CartesianTrajectory(Xstart, Xend, Tf, N, method): + """Computes a trajectory as a list of N SE(3) matrices corresponding to + the origin of the end-effector frame following a straight line + + :param Xstart: The initial end-effector configuration + :param Xend: The final end-effector configuration + :param Tf: Total time of the motion in seconds from rest to rest + :param N: The number of points N > 1 (Start and stop) in the discrete + representation of the trajectory + :param method: The time-scaling method, where 3 indicates cubic (third- + order polynomial) time scaling and 5 indicates quintic + (fifth-order polynomial) time scaling + :return: The discretized trajectory as a list of N matrices in SE(3) + separated in time by Tf/(N-1). The first in the list is Xstart + and the Nth is Xend + This function is similar to ScrewTrajectory, except the origin of the + end-effector frame follows a straight line, decoupled from the rotational + motion. + + Example Input: + Xstart = np.array([[1, 0, 0, 1], + [0, 1, 0, 0], + [0, 0, 1, 1], + [0, 0, 0, 1]]) + Xend = np.array([[0, 0, 1, 0.1], + [1, 0, 0, 0], + [0, 1, 0, 4.1], + [0, 0, 0, 1]]) + Tf = 5 + N = 4 + method = 5 + Output: + [np.array([[1, 0, 0, 1] + [0, 1, 0, 0] + [0, 0, 1, 1] + [0, 0, 0, 1]]), + np.array([[ 0.937, -0.214, 0.277, 0.811] + [ 0.277, 0.937, -0.214, 0] + [-0.214, 0.277, 0.937, 1.651] + [ 0, 0, 0, 1]]), + np.array([[ 0.277, -0.214, 0.937, 0.289] + [ 0.937, 0.277, -0.214, 0] + [-0.214, 0.937, 0.277, 3.449] + [ 0, 0, 0, 1]]), + np.array([[0, 0, 1, 0.1] + [1, 0, 0, 0] + [0, 1, 0, 4.1] + [0, 0, 0, 1]])] + """ + N = int(N) + timegap = Tf / (N - 1.0) + traj = [[None]] * N + Rstart, pstart = TransToRp(Xstart) + Rend, pend = TransToRp(Xend) + for i in range(N): + if method == 3: + s = CubicTimeScaling(Tf, timegap * i) + else: + s = QuinticTimeScaling(Tf, timegap * i) + traj[i] \ + = np.r_[np.c_[np.dot(Rstart, \ + MatrixExp3(MatrixLog3(np.dot(np.array(Rstart).T,Rend)) * s)), \ + s * np.array(pend) + (1 - s) * np.array(pstart)], \ + [[0, 0, 0, 1]]] + return traj + +''' +*** CHAPTER 11: ROBOT CONTROL *** +''' + +def ComputedTorque(thetalist, dthetalist, eint, g, Mlist, Glist, Slist, \ + thetalistd, dthetalistd, ddthetalistd, Kp, Ki, Kd): + """Computes the joint control torques at a particular time instant + + :param thetalist: n-vector of joint variables + :param dthetalist: n-vector of joint rates + :param eint: n-vector of the time-integral of joint errors + :param g: Gravity vector g + :param Mlist: List of link frames {i} relative to {i-1} at the home + position + :param Glist: Spatial inertia matrices Gi of the links + :param Slist: Screw axes Si of the joints in a space frame, in the format + of a matrix with axes as the columns + :param thetalistd: n-vector of reference joint variables + :param dthetalistd: n-vector of reference joint velocities + :param ddthetalistd: n-vector of reference joint accelerations + :param Kp: The feedback proportional gain (identical for each joint) + :param Ki: The feedback integral gain (identical for each joint) + :param Kd: The feedback derivative gain (identical for each joint) + :return: The vector of joint forces/torques computed by the feedback + linearizing controller at the current instant + + Example Input: + thetalist = np.array([0.1, 0.1, 0.1]) + dthetalist = np.array([0.1, 0.2, 0.3]) + eint = np.array([0.2, 0.2, 0.2]) + g = np.array([0, 0, -9.8]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + thetalistd = np.array([1.0, 1.0, 1.0]) + dthetalistd = np.array([2, 1.2, 2]) + ddthetalistd = np.array([0.1, 0.1, 0.1]) + Kp = 1.3 + Ki = 1.2 + Kd = 1.1 + Output: + np.array([133.00525246, -29.94223324, -3.03276856]) + """ + e = np.subtract(thetalistd, thetalist) + return np.dot(MassMatrix(thetalist, Mlist, Glist, Slist), \ + Kp * e + Ki * (np.array(eint) + e) \ + + Kd * np.subtract(dthetalistd, dthetalist)) \ + + InverseDynamics(thetalist, dthetalist, ddthetalistd, g, \ + [0, 0, 0, 0, 0, 0], Mlist, Glist, Slist) + +def SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, Glist, \ + Slist, thetamatd, dthetamatd, ddthetamatd, gtilde, \ + Mtildelist, Gtildelist, Kp, Ki, Kd, dt, intRes): + """Simulates the computed torque controller over a given desired + trajectory + + :param thetalist: n-vector of initial joint variables + :param dthetalist: n-vector of initial joint velocities + :param g: Actual gravity vector g + :param Ftipmat: An N x 6 matrix of spatial forces applied by the end- + effector (If there are no tip forces the user should + input a zero and a zero matrix will be used) + :param Mlist: Actual list of link frames i relative to i-1 at the home + position + :param Glist: Actual spatial inertia matrices Gi of the links + :param Slist: Screw axes Si of the joints in a space frame, in the format + of a matrix with axes as the columns + :param thetamatd: An Nxn matrix of desired joint variables from the + reference trajectory + :param dthetamatd: An Nxn matrix of desired joint velocities + :param ddthetamatd: An Nxn matrix of desired joint accelerations + :param gtilde: The gravity vector based on the model of the actual robot + (actual values given above) + :param Mtildelist: The link frame locations based on the model of the + actual robot (actual values given above) + :param Gtildelist: The link spatial inertias based on the model of the + actual robot (actual values given above) + :param Kp: The feedback proportional gain (identical for each joint) + :param Ki: The feedback integral gain (identical for each joint) + :param Kd: The feedback derivative gain (identical for each joint) + :param dt: The timestep between points on the reference trajectory + :param intRes: Integration resolution is the number of times integration + (Euler) takes places between each time step. Must be an + integer value greater than or equal to 1 + :return taumat: An Nxn matrix of the controllers commanded joint forces/ + torques, where each row of n forces/torques corresponds + to a single time instant + :return thetamat: An Nxn matrix of actual joint angles + The end of this function plots all the actual and desired joint angles + using matplotlib and random libraries. + + Example Input: + from __future__ import print_function + import numpy as np + from modern_robotics import JointTrajectory + thetalist = np.array([0.1, 0.1, 0.1]) + dthetalist = np.array([0.1, 0.2, 0.3]) + # Initialize robot description (Example with 3 links) + g = np.array([0, 0, -9.8]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + dt = 0.01 + # Create a trajectory to follow + thetaend = np.array([np.pi / 2, np.pi, 1.5 * np.pi]) + Tf = 1 + N = int(1.0 * Tf / dt) + method = 5 + traj = mr.JointTrajectory(thetalist, thetaend, Tf, N, method) + thetamatd = np.array(traj).copy() + dthetamatd = np.zeros((N, 3)) + ddthetamatd = np.zeros((N, 3)) + dt = Tf / (N - 1.0) + for i in range(np.array(traj).shape[0] - 1): + dthetamatd[i + 1, :] \ + = (thetamatd[i + 1, :] - thetamatd[i, :]) / dt + ddthetamatd[i + 1, :] \ + = (dthetamatd[i + 1, :] - dthetamatd[i, :]) / dt + # Possibly wrong robot description (Example with 3 links) + gtilde = np.array([0.8, 0.2, -8.8]) + Mhat01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.1], + [0, 0, 0, 1]]) + Mhat12 = np.array([[ 0, 0, 1, 0.3], + [ 0, 1, 0, 0.2], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + Mhat23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.2], + [0, 0, 1, 0.4], + [0, 0, 0, 1]]) + Mhat34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.2], + [0, 0, 0, 1]]) + Ghat1 = np.diag([0.1, 0.1, 0.1, 4, 4, 4]) + Ghat2 = np.diag([0.3, 0.3, 0.1, 9, 9, 9]) + Ghat3 = np.diag([0.1, 0.1, 0.1, 3, 3, 3]) + Gtildelist = np.array([Ghat1, Ghat2, Ghat3]) + Mtildelist = np.array([Mhat01, Mhat12, Mhat23, Mhat34]) + Ftipmat = np.ones((np.array(traj).shape[0], 6)) + Kp = 20 + Ki = 10 + Kd = 18 + intRes = 8 + taumat,thetamat \ + = mr.SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, \ + Glist, Slist, thetamatd, dthetamatd, \ + ddthetamatd, gtilde, Mtildelist, Gtildelist, \ + Kp, Ki, Kd, dt, intRes) + """ + Ftipmat = np.array(Ftipmat).T + thetamatd = np.array(thetamatd).T + dthetamatd = np.array(dthetamatd).T + ddthetamatd = np.array(ddthetamatd).T + m,n = np.array(thetamatd).shape + thetacurrent = np.array(thetalist).copy() + dthetacurrent = np.array(dthetalist).copy() + eint = np.zeros((m,1)).reshape(m,) + taumat = np.zeros(np.array(thetamatd).shape) + thetamat = np.zeros(np.array(thetamatd).shape) + for i in range(n): + taulist \ + = ComputedTorque(thetacurrent, dthetacurrent, eint, gtilde, \ + Mtildelist, Gtildelist, Slist, thetamatd[:, i], \ + dthetamatd[:, i], ddthetamatd[:, i], Kp, Ki, Kd) + for j in range(intRes): + ddthetalist \ + = ForwardDynamics(thetacurrent, dthetacurrent, taulist, g, \ + Ftipmat[:, i], Mlist, Glist, Slist) + thetacurrent, dthetacurrent \ + = EulerStep(thetacurrent, dthetacurrent, ddthetalist, \ + 1.0 * dt / intRes) + taumat[:, i] = taulist + thetamat[:, i] = thetacurrent + eint = np.add(eint, dt * np.subtract(thetamatd[:, i], thetacurrent)) + # Output using matplotlib to plot + try: + import matplotlib.pyplot as plt + except: + print('The result will not be plotted due to a lack of package matplotlib') + else: + links = np.array(thetamat).shape[0] + N = np.array(thetamat).shape[1] + Tf = N * dt + timestamp = np.linspace(0, Tf, N) + for i in range(links): + col = [np.random.uniform(0, 1), np.random.uniform(0, 1), + np.random.uniform(0, 1)] + plt.plot(timestamp, thetamat[i, :], "-", color=col, \ + label = ("ActualTheta" + str(i + 1))) + plt.plot(timestamp, thetamatd[i, :], ".", color=col, \ + label = ("DesiredTheta" + str(i + 1))) + plt.legend(loc = 'upper left') + plt.xlabel("Time") + plt.ylabel("Joint Angles") + plt.title("Plot of Actual and Desired Joint Angles") + plt.show() + taumat = np.array(taumat).T + thetamat = np.array(thetamat).T + return (taumat, thetamat) diff --git a/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/complexCommandTest.py b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/complexCommandTest.py new file mode 100644 index 0000000..f76fe2f --- /dev/null +++ b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/complexCommandTest.py @@ -0,0 +1,59 @@ +# This example illustrates how to execute complex commands from +# a remote API client. You can also use a similar construct for +# commands that are not directly supported by the remote API. +# +# Load the demo scene 'remoteApiCommandServerExample.ttt' in CoppeliaSim, then +# start the simulation and run this program. +# +# IMPORTANT: for each successful call to simxStart, there +# should be a corresponding call to simxFinish at the end! + +try: + import sim +except: + print ('--------------------------------------------------------------') + print ('"sim.py" could not be imported. This means very probably that') + print ('either "sim.py" or the remoteApi library could not be found.') + print ('Make sure both are in the same folder as this file,') + print ('or appropriately adjust the file "sim.py"') + print ('--------------------------------------------------------------') + print ('') + +import sys +import ctypes +print ('Program started') +sim.simxFinish(-1) # just in case, close all opened connections +clientID=sim.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to CoppeliaSim +if clientID!=-1: + print ('Connected to remote API server') + + # 1. First send a command to display a specific message in a dialog box: + emptyBuff = bytearray() + res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'displayText_function',[],[],['Hello world!'],emptyBuff,sim.simx_opmode_blocking) + if res==sim.simx_return_ok: + print ('Return string: ',retStrings[0]) # display the reply from CoppeliaSim (in this case, just a string) + else: + print ('Remote function call failed') + + # 2. Now create a dummy object at coordinate 0.1,0.2,0.3 with name 'MyDummyName': + res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'createDummy_function',[],[0.1,0.2,0.3],['MyDummyName'],emptyBuff,sim.simx_opmode_blocking) + if res==sim.simx_return_ok: + print ('Dummy handle: ',retInts[0]) # display the reply from CoppeliaSim (in this case, the handle of the created dummy) + else: + print ('Remote function call failed') + + # 3. Now send a code string to execute some random functions: + code="local octreeHandle=simCreateOctree(0.5,0,1)\n" \ + "simInsertVoxelsIntoOctree(octreeHandle,0,{0.1,0.1,0.1},{255,0,255})\n" \ + "return 'done'" + res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,"remoteApiCommandServer",sim.sim_scripttype_childscript,'executeCode_function',[],[],[code],emptyBuff,sim.simx_opmode_blocking) + if res==sim.simx_return_ok: + print ('Code execution returned: ',retStrings[0]) + else: + print ('Remote function call failed') + + # Now close the connection to CoppeliaSim: + sim.simxFinish(clientID) +else: + print ('Failed connecting to remote API server') +print ('Program ended') diff --git a/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/depth_image_encoding.py b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/depth_image_encoding.py new file mode 100644 index 0000000..5671298 --- /dev/null +++ b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/depth_image_encoding.py @@ -0,0 +1,229 @@ +"""Creates an image from a numpy array of floating point depth data. + +For details about the encoding see: +https://sites.google.com/site/brainrobotdata/home/depth-image-encoding + +Examples: + + depth_array is a 2D numpy array of floating point depth data in meters. + + depth_rgb = FloatArrayToRgbImage(depth_array) + depth_rgb is a PIL Image object containing the same data as 24-bit + integers encoded in the RGB bytes. + depth_rgb.save('image_file.png') - to save to a file. + + depth_array2 = ImageToFloatArray(depth_rgb) + depth_array2 is a 2D numpy array containing the same data as + depth_array up to the precision of the RGB image (1/256 mm). + + depth_gray = FloatArrayToGrayImage(depth_array) + depth_gray is a PIL Image object containing the same data rounded to + 8-bit integers. + depth_gray.save('image_file.jpg', quality=95) - to save to a file. + + depth_array3 = ImageToFloatArray(depth_gray) + depth_array3 is a 2D numpy array containing the same data as + depth_array up to the precision of the grayscale image (1 cm). + +The image conversions first scale and round the values and then pack +them into the desired type in a numpy array before converting the +array to a PIL Image object. The Image can be saved in any format. +We are using PNG for RGB and high quality JPEG for grayscale images. + +You can use different numeric types (e.g. np.uint16, np.int32), but +not all combinations of numeric type and image format are supported by +PIL or standard image viewers. + +""" + +import numpy as np +from PIL import Image +from skimage import img_as_ubyte +from skimage.color import grey2rgb + + +def ClipFloatValues(float_array, min_value, max_value): + """Clips values to the range [min_value, max_value]. + + First checks if any values are out of range and prints a message. + Then clips all values to the given range. + + Args: + float_array: 2D array of floating point values to be clipped. + min_value: Minimum value of clip range. + max_value: Maximum value of clip range. + + Returns: + The clipped array. + + """ + if float_array.min() < min_value or float_array.max() > max_value: + print('Image has out-of-range values [%f,%f] not in [%d,%d]', + float_array.min(), float_array.max(), min_value, max_value) + float_array = np.clip(float_array, min_value, max_value) + return float_array + + +DEFAULT_RGB_SCALE_FACTOR = 256000.0 + + +def FloatArrayToRgbImage(float_array, + scale_factor=DEFAULT_RGB_SCALE_FACTOR, + drop_blue=False): + """Convert a floating point array of values to an RGB image. + + Convert floating point values to a fixed point representation where + the RGB bytes represent a 24-bit integer. + R is the high order byte. + B is the low order byte. + The precision of the depth image is 1/256 mm. + + Floating point values are scaled so that the integer values cover + the representable range of depths. + + This image representation should only use lossless compression. + + Args: + float_array: Input array of floating point depth values in meters. + scale_factor: Scale value applied to all float values. + drop_blue: Zero out the blue channel to improve compression, results in 1mm + precision depth values. + + Returns: + 24-bit RGB PIL Image object representing depth values. + """ + float_array = np.squeeze(float_array) + # Scale the floating point array. + scaled_array = np.floor(float_array * scale_factor + 0.5) + # Convert the array to integer type and clip to representable range. + min_inttype = 0 + max_inttype = 2**24 - 1 + scaled_array = ClipFloatValues(scaled_array, min_inttype, max_inttype) + int_array = scaled_array.astype(np.uint32) + # Calculate: + # r = (f / 256) / 256 high byte + # g = (f / 256) % 256 middle byte + # b = f % 256 low byte + rg = np.divide(int_array, 256) + r = np.divide(rg, 256) + g = np.mod(rg, 256) + image_shape = int_array.shape + rgb_array = np.zeros((image_shape[0], image_shape[1], 3), dtype=np.uint8) + rgb_array[..., 0] = r + rgb_array[..., 1] = g + if not drop_blue: + # Calculate the blue channel and add it to the array. + b = np.mod(int_array, 256) + rgb_array[..., 2] = b + image_mode = 'RGB' + image = Image.fromarray(rgb_array, mode=image_mode) + return image + + +DEFAULT_GRAY_SCALE_FACTOR = {np.uint8: 100.0, + np.uint16: 1000.0, + np.int32: DEFAULT_RGB_SCALE_FACTOR} + + +def FloatArrayToGrayImage(float_array, scale_factor=None, image_dtype=np.uint8): + """Convert a floating point array of values to an RGB image. + + Convert floating point values to a fixed point representation with + the given bit depth. + + The precision of the depth image with default scale_factor is: + uint8: 1cm, with a range of [0, 2.55m] + uint16: 1mm, with a range of [0, 65.5m] + int32: 1/256mm, with a range of [0, 8388m] + + Right now, PIL turns uint16 images into a very strange format and + does not decode int32 images properly. Only uint8 works correctly. + + Args: + float_array: Input array of floating point depth values in meters. + scale_factor: Scale value applied to all float values. + image_dtype: Image datatype, which controls the bit depth of the grayscale + image. + + Returns: + Grayscale PIL Image object representing depth values. + + """ + # Ensure that we have a valid numeric type for the image. + if image_dtype == np.uint16: + image_mode = 'I;16' + elif image_dtype == np.int32: + image_mode = 'I' + else: + image_dtype = np.uint8 + image_mode = 'L' + if scale_factor is None: + scale_factor = DEFAULT_GRAY_SCALE_FACTOR[image_dtype] + # Scale the floating point array. + scaled_array = np.floor(float_array * scale_factor + 0.5) + # Convert the array to integer type and clip to representable range. + min_dtype = np.iinfo(image_dtype).min + max_dtype = np.iinfo(image_dtype).max + scaled_array = ClipFloatValues(scaled_array, min_dtype, max_dtype) + + image_array = scaled_array.astype(image_dtype) + image = Image.fromarray(image_array, mode=image_mode) + return image + + +def ImageToFloatArray(image, scale_factor=None): + """Recovers the depth values from an image. + + Reverses the depth to image conversion performed by FloatArrayToRgbImage or + FloatArrayToGrayImage. + + The image is treated as an array of fixed point depth values. Each + value is converted to float and scaled by the inverse of the factor + that was used to generate the Image object from depth values. If + scale_factor is specified, it should be the same value that was + specified in the original conversion. + + The result of this function should be equal to the original input + within the precision of the conversion. + + For details see https://sites.google.com/site/brainrobotdata/home/depth-image-encoding. + + Args: + image: Depth image output of FloatArrayTo[Format]Image. + scale_factor: Fixed point scale factor. + + Returns: + A 2D floating point numpy array representing a depth image. + + """ + image_array = np.array(image) + image_dtype = image_array.dtype + image_shape = image_array.shape + + channels = image_shape[2] if len(image_shape) > 2 else 1 + assert 2 <= len(image_shape) <= 3 + if channels == 3: + # RGB image needs to be converted to 24 bit integer. + float_array = np.sum(image_array * [65536, 256, 1], axis=2) + if scale_factor is None: + scale_factor = DEFAULT_RGB_SCALE_FACTOR + else: + if scale_factor is None: + scale_factor = DEFAULT_GRAY_SCALE_FACTOR[image_dtype.type] + float_array = image_array.astype(np.float32) + scaled_array = float_array / scale_factor + return scaled_array + + +def FloatArrayToRawRGB(im, min_value=0.0, max_value=1.0): + """Convert a grayscale image to rgb, no encoding. + + For proper display try matplotlib's rendering/conversion instead of this version. + Please be aware that this does not incorporate a proper color transform. + http://pillow.readthedocs.io/en/3.4.x/reference/Image.html#PIL.Image.Image.convert + https://en.wikipedia.org/wiki/Rec._601 + """ + im = img_as_ubyte(im) + if im.shape[-1] == 1: + im = grey2rgb(im) + return im diff --git a/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/pController.py b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/pController.py new file mode 100644 index 0000000..bdf55ae --- /dev/null +++ b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/pController.py @@ -0,0 +1,104 @@ +# Make sure to have CoppeliaSim running, with followig scene loaded: +# +# scenes/pControllerViaRemoteApi.ttt +# +# Do not launch simulation, but run this script +# +# The client side (i.e. this script) depends on: +# +# sim.py, simConst.py, and the remote API library available +# in programming/remoteApiBindings/lib/lib + +try: + import sim +except: + print ('--------------------------------------------------------------') + print ('"sim.py" could not be imported. This means very probably that') + print ('either "sim.py" or the remoteApi library could not be found.') + print ('Make sure both are in the same folder as this file,') + print ('or appropriately adjust the file "sim.py"') + print ('--------------------------------------------------------------') + print ('') + +import math + +class Client: + def __enter__(self): + self.intSignalName='legacyRemoteApiStepCounter' + self.stepCounter=0 + self.maxForce=100 + sim.simxFinish(-1) # just in case, close all opened connections + self.id=sim.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to CoppeliaSim + return self + + def __exit__(self,*err): + sim.simxFinish(-1) + +with Client() as client: + print("running") + + if client.id!=-1: + print ('Connected to remote API server') + + def stepSimulation(): + currentStep=client.stepCounter + sim.simxSynchronousTrigger(client.id); + while client.stepCounter==currentStep: + retCode,s=sim.simxGetIntegerSignal(client.id,client.intSignalName,sim.simx_opmode_buffer) + if retCode==sim.simx_return_ok: + client.stepCounter=s + + def getCurrentJointAngle(): + retCode=-1 + jointA=0 + while retCode!=sim.simx_return_ok: + retCode,jointA=sim.simxGetJointPosition(client.id,client.jointHandle,sim.simx_opmode_buffer) + return jointA + + def moveToAngle(targetAngle): + jointAngle=getCurrentJointAngle() + while abs(jointAngle-targetAngle)>0.1*math.pi/180: + stepSimulation() + jointAngle=getCurrentJointAngle() + vel=computeTargetVelocity(jointAngle,targetAngle) + sim.simxSetJointTargetVelocity(client.id,client.jointHandle,vel,sim.simx_opmode_oneshot) + sim.simxSetJointMaxForce(client.id,client.jointHandle,client.maxForce,sim.simx_opmode_oneshot) + + def computeTargetVelocity(jointAngle,targetAngle): + dynStepSize=0.005 + velUpperLimit=360*math.pi/180 + PID_P=0.1 + errorValue=targetAngle-jointAngle + sinAngle=math.sin(errorValue) + cosAngle=math.cos(errorValue) + errorValue=math.atan2(sinAngle,cosAngle) + ctrl=errorValue*PID_P + + # Calculate the velocity needed to reach the position in one dynamic time step: + velocity=ctrl/dynStepSize + if (velocity>velUpperLimit): + velocity=velUpperLimit + + if (velocity<-velUpperLimit): + velocity=-velUpperLimit + + return velocity + + # Start streaming client.intSignalName integer signal, that signals when a step is finished: + sim.simxGetIntegerSignal(client.id,client.intSignalName,sim.simx_opmode_streaming) + + res,client.jointHandle=sim.simxGetObjectHandle(client.id,'joint',sim.simx_opmode_blocking) + sim.simxSetJointTargetVelocity(client.id,client.jointHandle,360*math.pi/180,sim.simx_opmode_oneshot) + sim.simxGetJointPosition(client.id,client.jointHandle,sim.simx_opmode_streaming) + + # enable the synchronous mode on the client: + sim.simxSynchronous(client.id,True) + + sim.simxStartSimulation(client.id,sim.simx_opmode_oneshot) + + moveToAngle(45*math.pi/180) + moveToAngle(90*math.pi/180) + moveToAngle(-89*math.pi/180) #no -90, to avoid passing below + moveToAngle(0*math.pi/180) + + sim.simxStopSimulation(client.id,sim.simx_opmode_blocking) diff --git a/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/pathPlanningTest.py b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/pathPlanningTest.py new file mode 100644 index 0000000..772ab46 --- /dev/null +++ b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/pathPlanningTest.py @@ -0,0 +1,160 @@ +# This example illustrates how to use the path/motion +# planning functionality from a remote API client. +# +# Load the demo scene 'motionPlanningServerDemo.ttt' in CoppeliaSim +# then run this program. +# +# IMPORTANT: for each successful call to simxStart, there +# should be a corresponding call to simxFinish at the end! + +import sim + +print ('Program started') +sim.simxFinish(-1) # just in case, close all opened connections +clientID=sim.simxStart('127.0.0.1',19997,True,True,-500000,5) # Connect to CoppeliaSim, set a very large time-out for blocking commands +if clientID!=-1: + print ('Connected to remote API server') + + emptyBuff = bytearray() + + # Start the simulation: + sim.simxStartSimulation(clientID,sim.simx_opmode_oneshot_wait) + + # Load a robot instance: res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'loadRobot',[],[0,0,0,0],['d:/coppeliaRobotics/qrelease/release/test.ttm'],emptyBuff,sim.simx_opmode_oneshot_wait) + # robotHandle=retInts[0] + + # Retrieve some handles: + res,robotHandle=sim.simxGetObjectHandle(clientID,'IRB4600#',sim.simx_opmode_oneshot_wait) + res,target1=sim.simxGetObjectHandle(clientID,'testPose1#',sim.simx_opmode_oneshot_wait) + res,target2=sim.simxGetObjectHandle(clientID,'testPose2#',sim.simx_opmode_oneshot_wait) + res,target3=sim.simxGetObjectHandle(clientID,'testPose3#',sim.simx_opmode_oneshot_wait) + res,target4=sim.simxGetObjectHandle(clientID,'testPose4#',sim.simx_opmode_oneshot_wait) + + # Retrieve the poses (i.e. transformation matrices, 12 values, last row is implicit) of some dummies in the scene + res,retInts,target1Pose,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getObjectPose',[target1],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) + res,retInts,target2Pose,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getObjectPose',[target2],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) + res,retInts,target3Pose,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getObjectPose',[target3],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) + res,retInts,target4Pose,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getObjectPose',[target4],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) + + # Get the robot initial state: + res,retInts,robotInitialState,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getRobotState',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) + + # Some parameters: + approachVector=[0,0,1] # often a linear approach is required. This should also be part of the calculations when selecting an appropriate state for a given pose + maxConfigsForDesiredPose=10 # we will try to find 10 different states corresponding to the goal pose and order them according to distance from initial state + maxTrialsForConfigSearch=300 # a parameter needed for finding appropriate goal states + searchCount=2 # how many times OMPL will run for a given task + minConfigsForPathPlanningPath=400 # interpolation states for the OMPL path + minConfigsForIkPath=100 # interpolation states for the linear approach path + collisionChecking=1 # whether collision checking is on or off + + # Display a message: + res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'displayMessage',[],[],['Computing and executing several path planning tasks for a given goal pose.&&nSeveral goal states corresponding to the goal pose are tested.&&nFeasability of a linear approach is also tested. Collision detection is on.'],emptyBuff,sim.simx_opmode_oneshot_wait) + + # Do the path planning here (between a start state and a goal pose, including a linear approach phase): + inInts=[robotHandle,collisionChecking,minConfigsForIkPath,minConfigsForPathPlanningPath,maxConfigsForDesiredPose,maxTrialsForConfigSearch,searchCount] + inFloats=robotInitialState+target1Pose+approachVector + res,retInts,path,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'findPath_goalIsPose',inInts,inFloats,[],emptyBuff,sim.simx_opmode_oneshot_wait) + + if (res==0) and len(path)>0: + # The path could be in 2 parts: a path planning path, and a linear approach path: + part1StateCnt=retInts[0] + part2StateCnt=retInts[1] + path1=path[:part1StateCnt*6] + + # Visualize the first path: + res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'visualizePath',[robotHandle,255,0,255],path1,[],emptyBuff,sim.simx_opmode_oneshot_wait) + line1Handle=retInts[0] + + # Make the robot follow the path: + res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'runThroughPath',[robotHandle],path1,[],emptyBuff,sim.simx_opmode_oneshot_wait) + + # Wait until the end of the movement: + runningPath=True + while runningPath: + res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'isRunningThroughPath',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) + runningPath=retInts[0]==1 + + path2=path[part1StateCnt*6:] + + # Visualize the second path (the linear approach): + res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'visualizePath',[robotHandle,0,255,0],path2,[],emptyBuff,sim.simx_opmode_oneshot_wait) + line2Handle=retInts[0] + + # Make the robot follow the path: + res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'runThroughPath',[robotHandle],path2,[],emptyBuff,sim.simx_opmode_oneshot_wait) + + # Wait until the end of the movement: + runningPath=True + while runningPath: + res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'isRunningThroughPath',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) + runningPath=retInts[0]==1 + + # Clear the paths visualizations: + res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'removeLine',[line1Handle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) + res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'removeLine',[line2Handle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) + + # Get the robot current state: + res,retInts,robotCurrentConfig,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getRobotState',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) + + # Display a message: + res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'displayMessage',[],[],['Computing and executing several path planning tasks for a given goal state. Collision detection is on.'],emptyBuff,sim.simx_opmode_oneshot_wait) + + # Do the path planning here (between a start state and a goal state): + inInts=[robotHandle,collisionChecking,minConfigsForPathPlanningPath,searchCount] + inFloats=robotCurrentConfig+robotInitialState + res,retInts,path,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'findPath_goalIsState',inInts,inFloats,[],emptyBuff,sim.simx_opmode_oneshot_wait) + + if (res==0) and len(path)>0: + # Visualize the path: + res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'visualizePath',[robotHandle,255,0,255],path,[],emptyBuff,sim.simx_opmode_oneshot_wait) + lineHandle=retInts[0] + + # Make the robot follow the path: + res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'runThroughPath',[robotHandle],path,[],emptyBuff,sim.simx_opmode_oneshot_wait) + + # Wait until the end of the movement: + runningPath=True + while runningPath: + res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'isRunningThroughPath',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) + runningPath=retInts[0]==1 + + # Clear the path visualization: + res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'removeLine',[lineHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) + + # Collision checking off: + collisionChecking=0 + + # Display a message: + res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'displayMessage',[],[],['Computing and executing several linear paths, going through several waypoints. Collision detection is OFF.'],emptyBuff,sim.simx_opmode_oneshot_wait) + + # Find a linear path that runs through several poses: + inInts=[robotHandle,collisionChecking,minConfigsForIkPath] + inFloats=robotInitialState+target2Pose+target1Pose+target3Pose+target4Pose + res,retInts,path,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'findIkPath',inInts,inFloats,[],emptyBuff,sim.simx_opmode_oneshot_wait) + + if (res==0) and len(path)>0: + # Visualize the path: + res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'visualizePath',[robotHandle,0,255,255],path,[],emptyBuff,sim.simx_opmode_oneshot_wait) + line1Handle=retInts[0] + + # Make the robot follow the path: + res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'runThroughPath',[robotHandle],path,[],emptyBuff,sim.simx_opmode_oneshot_wait) + + # Wait until the end of the movement: + runningPath=True + while runningPath: + res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'isRunningThroughPath',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) + runningPath=retInts[0]==1 + + # Clear the path visualization: + res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'removeLine',[line1Handle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) + + # Stop simulation: + sim.simxStopSimulation(clientID,sim.simx_opmode_oneshot_wait) + + # Now close the connection to CoppeliaSim: + sim.simxFinish(clientID) +else: + print ('Failed connecting to remote API server') +print ('Program ended') diff --git a/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/ply.py b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/ply.py new file mode 100644 index 0000000..fb2caa2 --- /dev/null +++ b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/ply.py @@ -0,0 +1,83 @@ +# MIT LICENSE +# https://github.com/bponsler/kinectToPly/blob/master/kinectToPly.py +import numpy as np + + +class Ply(object): + '''The Ply class provides the ability to write a point cloud represented + by two arrays: an array of points (num points, 3), and an array of colors + (num points, 3) to a PLY file. + ''' + + def __init__(self, points, colors): + ''' + * points -- The matrix of points (num points, 3) + * colors -- The matrix of colors (num points, 3) + ''' + self.__points = points + self.__colors = colors + + def write(self, filename): + '''Write the point cloud data to a PLY file of the given name. + * filename -- The PLY file + ''' + # Write the headers + lines = self.__getLinesForHeader() + + fd = open(filename, "w") + for line in lines: + fd.write("%s\n" % line) + + # Write the points + self.__writePoints(fd, self.__points, self.__colors) + + fd.close() + + def __getLinesForHeader(self): + '''Get the list of lines for the PLY header.''' + lines = [ + "ply", + "format ascii 1.0", + "comment generated by: kinectToPly", + "element vertex %s" % len(self.__points), + "property float x", + "property float y", + "property float z", + "property uchar red", + "property uchar green", + "property uchar blue", + "end_header", + ] + + return lines + + def __writePoints(self, fd, points, colors): + '''Write the point cloud points to a file. + * fd -- The file descriptor + * points -- The matrix of points (num points, 3) + * colors -- The matrix of colors (num points, 3) + ''' + # Stack the two arrays together + stacked = np.column_stack((points, colors)) + + # Write the array to the file + np.savetxt( + fd, + stacked, + delimiter='\n', + fmt="%f %f %f %d %d %d") + + +def write_xyz_rgb_as_ply(point_cloud, rgb_image, path): + """Write a point cloud with associated rgb image to a ply file + + # Arguments + + point_cloud: xyz point cloud in format [height, width, channels] + rgb_image: uint8 image in format [height, width, channels] + path: Where to save the file, ex: '/path/to/folder/file.ply' + """ + xyz = point_cloud.reshape([point_cloud.size/3, 3]) + rgb = np.squeeze(rgb_image).reshape([point_cloud.size/3, 3]) + ply = Ply(xyz, rgb) + ply.write(path) diff --git a/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/readMe.txt b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/readMe.txt new file mode 100644 index 0000000..29933ef --- /dev/null +++ b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/readMe.txt @@ -0,0 +1,6 @@ +Make sure you have following files in your directory, in order to run the various examples: + +1. sim.py +2. simConst.py +3. the appropriate remote API library: "remoteApi.dll" (Windows), "remoteApi.dylib" (Mac) or "remoteApi.so" (Linux) +4. simpleTest.py (or any other example file) \ No newline at end of file diff --git a/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/remoteApi.dll b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/remoteApi.dll new file mode 100644 index 0000000..d9fb676 Binary files /dev/null and b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/remoteApi.dll differ diff --git a/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/sendMovementSequence-mov.py b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/sendMovementSequence-mov.py new file mode 100644 index 0000000..68fe8ed --- /dev/null +++ b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/sendMovementSequence-mov.py @@ -0,0 +1,103 @@ +# Make sure to have CoppeliaSim running, with followig scene loaded: +# +# scenes/movementViaRemoteApi.ttt +# +# Do not launch simulation, then run this script +# +# The client side (i.e. this script) depends on: +# +# sim.py, simConst.py, and the remote API library available +# in programming/remoteApiBindings/lib/lib +# Additionally you will need the python math and msgpack modules + +try: + import sim +except: + print ('--------------------------------------------------------------') + print ('"sim.py" could not be imported. This means very probably that') + print ('either "sim.py" or the remoteApi library could not be found.') + print ('Make sure both are in the same folder as this file,') + print ('or appropriately adjust the file "sim.py"') + print ('--------------------------------------------------------------') + print ('') + +import math +import msgpack + +print ('Program started') +sim.simxFinish(-1) # just in case, close all opened connections +clientID=sim.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to CoppeliaSim +if clientID!=-1: + print ('Connected to remote API server') + + executedMovId='notReady' + + targetArm='threadedBlueArm' + #targetArm='nonThreadedRedArm' + + stringSignalName=targetArm+'_executedMovId' + + def waitForMovementExecuted(id): + global executedMovId + global stringSignalName + while executedMovId!=id: + retCode,s=sim.simxGetStringSignal(clientID,stringSignalName,sim.simx_opmode_buffer) + if retCode==sim.simx_return_ok: + executedMovId=s + + # Start streaming stringSignalName string signal: + sim.simxGetStringSignal(clientID,stringSignalName,sim.simx_opmode_streaming) + + # Set-up some movement variables: + mVel=100*math.pi/180 + mAccel=150*math.pi/180 + maxVel=[mVel,mVel,mVel,mVel,mVel,mVel] + maxAccel=[mAccel,mAccel,mAccel,mAccel,mAccel,mAccel] + targetVel=[0,0,0,0,0,0] + + # Start simulation: + sim.simxStartSimulation(clientID,sim.simx_opmode_blocking) + + # Wait until ready: + waitForMovementExecuted('ready') + + # Send first movement sequence: + targetConfig=[90*math.pi/180,90*math.pi/180,-90*math.pi/180,90*math.pi/180,90*math.pi/180,90*math.pi/180] + movementData={"id":"movSeq1","type":"mov","targetConfig":targetConfig,"targetVel":targetVel,"maxVel":maxVel,"maxAccel":maxAccel} + packedMovementData=msgpack.packb(movementData) + sim.simxCallScriptFunction(clientID,targetArm,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) + + # Execute first movement sequence: + sim.simxCallScriptFunction(clientID,targetArm,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq1',sim.simx_opmode_oneshot) + + # Wait until above movement sequence finished executing: + waitForMovementExecuted('movSeq1') + + # Send second and third movement sequence, where third one should execute immediately after the second one: + targetConfig=[-90*math.pi/180,45*math.pi/180,90*math.pi/180,135*math.pi/180,90*math.pi/180,90*math.pi/180] + targetVel=[-60*math.pi/180,-20*math.pi/180,0,0,0,0] + movementData={"id":"movSeq2","type":"mov","targetConfig":targetConfig,"targetVel":targetVel,"maxVel":maxVel,"maxAccel":maxAccel} + packedMovementData=msgpack.packb(movementData) + sim.simxCallScriptFunction(clientID,targetArm,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) + targetConfig=[0,0,0,0,0,0] + targetVel=[0,0,0,0,0,0] + movementData={"id":"movSeq3","type":"mov","targetConfig":targetConfig,"targetVel":targetVel,"maxVel":maxVel,"maxAccel":maxAccel} + packedMovementData=msgpack.packb(movementData) + sim.simxCallScriptFunction(clientID,targetArm,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) + + # Execute second and third movement sequence: + sim.simxCallScriptFunction(clientID,targetArm,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq2',sim.simx_opmode_oneshot) + sim.simxCallScriptFunction(clientID,targetArm,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq3',sim.simx_opmode_oneshot) + + # Wait until above 2 movement sequences finished executing: + waitForMovementExecuted('movSeq3') + sim.simxStopSimulation(clientID,sim.simx_opmode_blocking) + sim.simxGetStringSignal(clientID,stringSignalName,sim.simx_opmode_discontinue) + sim.simxGetPingTime(clientID) + + # Now close the connection to CoppeliaSim: + sim.simxFinish(clientID) +else: + print ('Failed connecting to remote API server') +print ('Program ended') + diff --git a/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/sendMovementSequence-pts.py b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/sendMovementSequence-pts.py new file mode 100644 index 0000000..6d1e5ea --- /dev/null +++ b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/sendMovementSequence-pts.py @@ -0,0 +1,96 @@ +# Make sure to have CoppeliaSim running, with followig scene loaded: +# +# scenes/movementViaRemoteApi.ttt +# +# Do not launch simulation, then run this script +# +# The client side (i.e. this script) depends on: +# +# sim.py, simConst.py, and the remote API library available +# in programming/remoteApiBindings/lib/lib +# Additionally you will need the python math and msgpack modules + +try: + import sim +except: + print ('--------------------------------------------------------------') + print ('"sim.py" could not be imported. This means very probably that') + print ('either "sim.py" or the remoteApi library could not be found.') + print ('Make sure both are in the same folder as this file,') + print ('or appropriately adjust the file "sim.py"') + print ('--------------------------------------------------------------') + print ('') + +import time +import math +import msgpack + +print ('Program started') +sim.simxFinish(-1) # just in case, close all opened connections +clientID=sim.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to CoppeliaSim +if clientID!=-1: + print ('Connected to remote API server') + + executedMovId='notReady' + + targetArm='threadedBlueArm' + #targetArm='nonThreadedRedArm' + + stringSignalName=targetArm+'_executedMovId' + + def waitForMovementExecuted(id): + global executedMovId + global stringSignalName + while executedMovId!=id: + retCode,s=sim.simxGetStringSignal(clientID,stringSignalName,sim.simx_opmode_buffer) + if retCode==sim.simx_return_ok: + executedMovId=s + + # Start streaming stringSignalName string signal: + sim.simxGetStringSignal(clientID,stringSignalName,sim.simx_opmode_streaming) + + # Set-up some movement variables: + times=[0.000,0.050,0.100,0.150,0.200,0.250,0.300,0.350,0.400,0.450,0.500,0.550,0.600,0.650,0.700,0.750,0.800,0.850,0.900,0.950,1.000,1.050,1.100,1.150,1.200,1.250,1.300,1.350,1.400,1.450,1.500,1.550,1.600,1.650,1.700,1.750,1.800,1.850] + j1=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.360,0.431,0.506,0.587,0.669,0.753,0.838,0.923,1.008,1.091,1.170,1.243,1.308,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.569,1.570,1.571,1.571,1.571] + j2=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.140,0.185,0.237,0.296,0.361,0.431,0.507,0.587,0.670,0.754,0.838,0.924,1.009,1.092,1.171,1.243,1.308,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.569,1.570,1.571,1.571,1.571] + j3=[0.000,0.000,-0.002,-0.009,-0.022,-0.042,-0.068,-0.100,-0.139,-0.185,-0.237,-0.296,-0.361,-0.433,-0.511,-0.595,-0.681,-0.767,-0.854,-0.942,-1.027,-1.107,-1.182,-1.249,-1.311,-1.365,-1.414,-1.455,-1.491,-1.519,-1.541,-1.557,-1.566,-1.569,-1.570,-1.571,-1.571,-1.571] + j4=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.027,1.107,1.181,1.249,1.310,1.365,1.413,1.455,1.490,1.519,1.541,1.556,1.565,1.569,1.570,1.570,1.570,1.571] + j5=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.028,1.108,1.182,1.250,1.311,1.366,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.569,1.570,1.571,1.571,1.571] + j6=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.027,1.108,1.182,1.249,1.311,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.569,1.570,1.571,1.571,1.571] + + times=[0.000,0.050,0.100,0.150,0.200,0.250,0.300,0.350,0.400,0.450,0.500,0.550,0.600,0.650,0.700,0.750,0.800,0.850,0.900,0.950,1.000,1.050,1.100,1.150,1.200,1.250,1.300,1.350,1.400,1.450,1.500,1.550,1.600,1.650,1.700,1.750,1.800,1.850,1.900,1.950,2.000,2.050,2.100,2.150,2.200,2.250,2.300,2.350,2.400,2.450,2.500,2.550,2.600,2.650,2.700,2.750,2.800,2.850,2.900,2.950,3.000,3.050,3.100,3.150,3.200,3.250,3.300,3.350,3.400,3.450,3.500,3.550,3.600,3.650,3.700,3.750,3.800,3.850,3.900,3.950,4.000,4.050,4.100,4.150,4.200,4.250,4.300,4.350,4.400,4.450,4.500,4.550,4.600,4.650,4.700,4.750,4.800,4.850,4.900,4.950,5.000,5.050,5.100,5.150,5.200,5.250,5.300,5.350,5.400,5.450,5.500,5.550,5.600,5.650,5.700,5.750,5.800,5.850,5.900,5.950,6.000,6.050,6.100,6.150,6.200,6.250,6.300,6.350] + j1=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.360,0.431,0.506,0.587,0.669,0.753,0.838,0.923,1.008,1.091,1.170,1.243,1.308,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.564,1.556,1.543,1.524,1.498,1.465,1.426,1.380,1.328,1.270,1.205,1.136,1.065,0.994,0.922,0.849,0.777,0.705,0.632,0.560,0.487,0.415,0.342,0.270,0.197,0.125,0.053,-0.020,-0.092,-0.165,-0.237,-0.309,-0.382,-0.454,-0.527,-0.599,-0.671,-0.744,-0.816,-0.888,-0.961,-1.033,-1.106,-1.178,-1.250,-1.323,-1.394,-1.462,-1.523,-1.556,-1.595,-1.632,-1.664,-1.690,-1.709,-1.723,-1.729,-1.730,-1.723,-1.710,-1.691,-1.665,-1.632,-1.593,-1.548,-1.495,-1.437,-1.372,-1.302,-1.226,-1.146,-1.064,-0.980,-0.895,-0.810,-0.724,-0.638,-0.552,-0.469,-0.390,-0.318,-0.254,-0.199,-0.151,-0.110,-0.076,-0.048,-0.027,-0.012,-0.004,-0.001,-0.001,-0.000,-0.000,-0.000] + j2=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.140,0.185,0.237,0.296,0.361,0.431,0.507,0.587,0.670,0.754,0.838,0.924,1.009,1.092,1.171,1.243,1.308,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.564,1.557,1.544,1.529,1.513,1.497,1.481,1.465,1.449,1.432,1.416,1.400,1.384,1.367,1.351,1.335,1.319,1.303,1.286,1.270,1.254,1.238,1.221,1.205,1.189,1.173,1.157,1.140,1.124,1.108,1.092,1.075,1.059,1.043,1.027,1.010,0.994,0.978,0.961,0.945,0.929,0.913,0.896,0.880,0.864,0.848,0.831,0.815,0.799,0.786,0.769,0.749,0.730,0.710,0.689,0.669,0.649,0.629,0.609,0.589,0.569,0.548,0.528,0.508,0.488,0.468,0.448,0.427,0.407,0.387,0.367,0.347,0.327,0.306,0.286,0.266,0.246,0.226,0.206,0.186,0.166,0.146,0.125,0.105,0.084,0.064,0.044,0.025,0.012,0.004,0.001,0.000,0.000,0.000,0.000] + j3=[0.000,0.000,-0.002,-0.009,-0.022,-0.042,-0.068,-0.100,-0.139,-0.185,-0.237,-0.296,-0.361,-0.433,-0.511,-0.595,-0.681,-0.767,-0.854,-0.942,-1.027,-1.107,-1.182,-1.249,-1.311,-1.365,-1.414,-1.455,-1.491,-1.519,-1.541,-1.557,-1.566,-1.564,-1.556,-1.543,-1.524,-1.498,-1.465,-1.426,-1.381,-1.328,-1.270,-1.205,-1.133,-1.055,-0.971,-0.885,-0.798,-0.711,-0.624,-0.537,-0.450,-0.362,-0.275,-0.188,-0.101,-0.013,0.074,0.161,0.249,0.336,0.423,0.510,0.598,0.685,0.772,0.859,0.947,1.032,1.112,1.186,1.253,1.314,1.369,1.416,1.458,1.492,1.521,1.542,1.557,1.566,1.564,1.557,1.544,1.524,1.498,1.466,1.427,1.383,1.338,1.293,1.247,1.201,1.155,1.110,1.064,1.018,0.972,0.926,0.881,0.835,0.789,0.743,0.697,0.652,0.606,0.560,0.514,0.468,0.423,0.377,0.331,0.285,0.239,0.194,0.149,0.109,0.076,0.048,0.027,0.012,0.004,0.002,0.001,0.000,0.000,0.000] + j4=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.027,1.107,1.181,1.249,1.310,1.365,1.413,1.455,1.490,1.519,1.541,1.556,1.565,1.567,1.574,1.587,1.603,1.619,1.636,1.653,1.670,1.686,1.703,1.720,1.737,1.754,1.771,1.788,1.805,1.822,1.839,1.856,1.873,1.890,1.907,1.923,1.940,1.957,1.974,1.991,2.008,2.025,2.042,2.059,2.076,2.093,2.110,2.127,2.144,2.161,2.178,2.195,2.212,2.229,2.246,2.263,2.280,2.297,2.314,2.331,2.344,2.352,2.350,2.343,2.330,2.310,2.284,2.252,2.212,2.167,2.115,2.056,1.991,1.919,1.841,1.760,1.679,1.597,1.515,1.433,1.350,1.268,1.186,1.104,1.022,0.940,0.858,0.776,0.694,0.612,0.530,0.452,0.379,0.312,0.252,0.198,0.151,0.110,0.076,0.048,0.027,0.012,0.004,0.002,0.001,0.000,0.000,0.000] + j5=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.028,1.108,1.182,1.250,1.311,1.366,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.571,1.571,1.568,1.561,1.548,1.529,1.503,1.470,1.431,1.388,1.342,1.297,1.251,1.205,1.159,1.113,1.067,1.021,0.975,0.929,0.883,0.837,0.791,0.745,0.699,0.653,0.607,0.561,0.516,0.470,0.424,0.378,0.332,0.286,0.240,0.194,0.149,0.109,0.076,0.048,0.027,0.012,0.004,0.002,0.001,0.000,0.000,0.000] + j6=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.027,1.108,1.182,1.249,1.311,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.571,1.571,1.571,1.569,1.561,1.548,1.529,1.503,1.470,1.431,1.388,1.343,1.297,1.251,1.205,1.159,1.113,1.067,1.021,0.975,0.929,0.883,0.837,0.791,0.745,0.699,0.653,0.607,0.561,0.515,0.470,0.424,0.378,0.332,0.286,0.240,0.194,0.149,0.109,0.076,0.048,0.027,0.012,0.004,0.002,0.001,0.000,0.000,0.000] + + # Start simulation: + sim.simxStartSimulation(clientID,sim.simx_opmode_blocking) + + # Wait until ready: + waitForMovementExecuted('ready') + + # Send the movement sequence: + targetConfig=[90*math.pi/180,90*math.pi/180,-90*math.pi/180,90*math.pi/180,90*math.pi/180,90*math.pi/180] + movementData={"id":"movSeq1","type":"pts","times":times,"j1":j1,"j2":j2,"j3":j3,"j4":j4,"j5":j5,"j6":j6} + packedMovementData=msgpack.packb(movementData) + sim.simxCallScriptFunction(clientID,targetArm,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) + + # Execute movement sequence: + sim.simxCallScriptFunction(clientID,targetArm,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq1',sim.simx_opmode_oneshot) + + # Wait until above movement sequence finished executing: + waitForMovementExecuted('movSeq1') + + sim.simxStopSimulation(clientID,sim.simx_opmode_blocking) + sim.simxGetIntegerSignal(clientID,integerSignalName,sim.simx_opmode_discontinue) + sim.simxGetPingTime(clientID) + + # Now close the connection to CoppeliaSim: + sim.simxFinish(clientID) +else: + print ('Failed connecting to remote API server') +print ('Program ended') + diff --git a/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/sendSimultan2MovementSequences-mov.py b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/sendSimultan2MovementSequences-mov.py new file mode 100644 index 0000000..a251261 --- /dev/null +++ b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/sendSimultan2MovementSequences-mov.py @@ -0,0 +1,129 @@ +# Make sure to have CoppeliaSim running, with followig scene loaded: +# +# scenes/movementViaRemoteApi.ttt +# +# Do not launch simulation, then run this script +# +# The client side (i.e. this script) depends on: +# +# sim.py, simConst.py, and the remote API library available +# in programming/remoteApiBindings/lib/lib +# Additionally you will need the python math and msgpack modules + +try: + import sim +except: + print ('--------------------------------------------------------------') + print ('"sim.py" could not be imported. This means very probably that') + print ('either "sim.py" or the remoteApi library could not be found.') + print ('Make sure both are in the same folder as this file,') + print ('or appropriately adjust the file "sim.py"') + print ('--------------------------------------------------------------') + print ('') + +import math +import msgpack + +class Client: + def __enter__(self): + self.executedMovId1='notReady' + self.executedMovId2='notReady' + sim.simxFinish(-1) # just in case, close all opened connections + self.id=sim.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to CoppeliaSim + return self + + def __exit__(self,*err): + sim.simxFinish(-1) + print ('Program ended') + +with Client() as client: + print("running") + + if client.id!=-1: + print ('Connected to remote API server') + + targetArm1='threadedBlueArm' + targetArm2='nonThreadedRedArm' + + client.stringSignalName1=targetArm1+'_executedMovId' + client.stringSignalName2=targetArm2+'_executedMovId' + + def waitForMovementExecuted1(id): + while client.executedMovId1!=id: + retCode,s=sim.simxGetStringSignal(client.id,client.stringSignalName1,sim.simx_opmode_buffer) + if retCode==sim.simx_return_ok: + client.executedMovId1=s + + def waitForMovementExecuted2(id): + while client.executedMovId2!=id: + retCode,s=sim.simxGetStringSignal(client.id,client.stringSignalName2,sim.simx_opmode_buffer) + if retCode==sim.simx_return_ok: + client.executedMovId2=s + + # Start streaming client.stringSignalName1 and client.stringSignalName2 string signals: + sim.simxGetStringSignal(client.id,client.stringSignalName1,sim.simx_opmode_streaming) + sim.simxGetStringSignal(client.id,client.stringSignalName2,sim.simx_opmode_streaming) + + # Set-up some movement variables: + mVel=100*math.pi/180 + mAccel=150*math.pi/180 + maxVel=[mVel,mVel,mVel,mVel,mVel,mVel] + maxAccel=[mAccel,mAccel,mAccel,mAccel,mAccel,mAccel] + targetVel=[0,0,0,0,0,0] + + # Start simulation: + sim.simxStartSimulation(client.id,sim.simx_opmode_blocking) + + # Wait until ready: + waitForMovementExecuted1('ready') + waitForMovementExecuted1('ready') + + # Send first movement sequence: + targetConfig=[90*math.pi/180,90*math.pi/180,-90*math.pi/180,90*math.pi/180,90*math.pi/180,90*math.pi/180] + movementData={"id":"movSeq1","type":"mov","targetConfig":targetConfig,"targetVel":targetVel,"maxVel":maxVel,"maxAccel":maxAccel} + packedMovementData=msgpack.packb(movementData) + sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) + sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) + + # Execute first movement sequence: + sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq1',sim.simx_opmode_oneshot) + sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq1',sim.simx_opmode_oneshot) + + # Wait until above movement sequence finished executing: + waitForMovementExecuted1('movSeq1') + waitForMovementExecuted1('movSeq1') + + # Send second and third movement sequence, where third one should execute immediately after the second one: + targetConfig=[-90*math.pi/180,45*math.pi/180,90*math.pi/180,135*math.pi/180,90*math.pi/180,90*math.pi/180] + targetVel=[-60*math.pi/180,-20*math.pi/180,0,0,0,0] + movementData={"id":"movSeq2","type":"mov","targetConfig":targetConfig,"targetVel":targetVel,"maxVel":maxVel,"maxAccel":maxAccel} + packedMovementData=msgpack.packb(movementData) + sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) + sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) + targetConfig=[0,0,0,0,0,0] + targetVel=[0,0,0,0,0,0] + movementData={"id":"movSeq3","type":"mov","targetConfig":targetConfig,"targetVel":targetVel,"maxVel":maxVel,"maxAccel":maxAccel} + packedMovementData=msgpack.packb(movementData) + sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) + sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) + + # Execute second and third movement sequence: + sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq2',sim.simx_opmode_oneshot) + sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq2',sim.simx_opmode_oneshot) + sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq3',sim.simx_opmode_oneshot) + sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq3',sim.simx_opmode_oneshot) + + # Wait until above 2 movement sequences finished executing: + waitForMovementExecuted1('movSeq3') + waitForMovementExecuted1('movSeq3') + + sim.simxStopSimulation(client.id,sim.simx_opmode_blocking) + sim.simxGetStringSignal(client.id,client.stringSignalName1,sim.simx_opmode_discontinue) + sim.simxGetStringSignal(client.id,client.stringSignalName2,sim.simx_opmode_discontinue) + sim.simxGetPingTime(client.id) + + # Now close the connection to CoppeliaSim: + sim.simxFinish(client.id) + else: + print ('Failed connecting to remote API server') + diff --git a/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/sim.py b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/sim.py new file mode 100644 index 0000000..b77fdf0 --- /dev/null +++ b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/sim.py @@ -0,0 +1,1493 @@ +import platform +import struct +import sys +import os +import ctypes as ct +from simConst import * + +#load library +libsimx = None +try: + file_extension = '.so' + if platform.system() =='cli': + file_extension = '.dll' + elif platform.system() =='Windows': + file_extension = '.dll' + elif platform.system() == 'Darwin': + file_extension = '.dylib' + else: + file_extension = '.so' + libfullpath = os.path.join(os.path.dirname(__file__), 'remoteApi' + file_extension) + libsimx = ct.CDLL(libfullpath) +except: + print ('----------------------------------------------------') + print ('The remoteApi library could not be loaded. Make sure') + print ('it is located in the same folder as "sim.py", or') + print ('appropriately adjust the file "sim.py"') + print ('----------------------------------------------------') + print ('') + +#ctypes wrapper prototypes +c_GetJointPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetJointPosition", libsimx)) +c_SetJointPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetJointPosition", libsimx)) +c_GetJointMatrix = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetJointMatrix", libsimx)) +c_SetSphericalJointMatrix = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetSphericalJointMatrix", libsimx)) +c_SetJointTargetVelocity = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetJointTargetVelocity", libsimx)) +c_SetJointTargetPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetJointTargetPosition", libsimx)) +c_GetJointForce = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetJointForce", libsimx)) +c_GetJointMaxForce = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetJointMaxForce", libsimx)) +c_SetJointForce = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetJointMaxForce", libsimx)) +c_SetJointMaxForce = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetJointMaxForce", libsimx)) +c_ReadForceSensor = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.POINTER(ct.c_float), ct.POINTER(ct.c_float), ct.c_int32)(("simxReadForceSensor", libsimx)) +c_BreakForceSensor = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxBreakForceSensor", libsimx)) +c_ReadVisionSensor = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.POINTER(ct.POINTER(ct.c_float)), ct.POINTER(ct.POINTER(ct.c_int32)), ct.c_int32)(("simxReadVisionSensor", libsimx)) +c_GetObjectHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectHandle", libsimx)) +c_GetVisionSensorImage = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_byte)), ct.c_ubyte, ct.c_int32)(("simxGetVisionSensorImage", libsimx)) +c_SetVisionSensorImage = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_byte), ct.c_int32, ct.c_ubyte, ct.c_int32)(("simxSetVisionSensorImage", libsimx)) +c_GetVisionSensorDepthBuffer= ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_float)), ct.c_int32)(("simxGetVisionSensorDepthBuffer", libsimx)) +c_GetObjectChild = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectChild", libsimx)) +c_GetObjectParent = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectParent", libsimx)) +c_ReadProximitySensor = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.POINTER(ct.c_float), ct.POINTER(ct.c_int32), ct.POINTER(ct.c_float), ct.c_int32)(("simxReadProximitySensor", libsimx)) +c_LoadModel = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_ubyte, ct.POINTER(ct.c_int32), ct.c_int32)(("simxLoadModel", libsimx)) +c_LoadUI = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_ubyte, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)), ct.c_int32)(("simxLoadUI", libsimx)) +c_LoadScene = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_ubyte, ct.c_int32)(("simxLoadScene", libsimx)) +c_StartSimulation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32)(("simxStartSimulation", libsimx)) +c_PauseSimulation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32)(("simxPauseSimulation", libsimx)) +c_StopSimulation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32)(("simxStopSimulation", libsimx)) +c_GetUIHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetUIHandle", libsimx)) +c_GetUISlider = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetUISlider", libsimx)) +c_SetUISlider = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetUISlider", libsimx)) +c_GetUIEventButton = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetUIEventButton", libsimx)) +c_GetUIButtonProperty = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetUIButtonProperty", libsimx)) +c_SetUIButtonProperty = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetUIButtonProperty", libsimx)) +c_AddStatusbarMessage = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxAddStatusbarMessage", libsimx)) +c_AuxiliaryConsoleOpen = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.c_int32), ct.POINTER(ct.c_float), ct.POINTER(ct.c_float), ct.POINTER(ct.c_int32), ct.c_int32)(("simxAuxiliaryConsoleOpen", libsimx)) +c_AuxiliaryConsoleClose = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxAuxiliaryConsoleClose", libsimx)) +c_AuxiliaryConsolePrint = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxAuxiliaryConsolePrint", libsimx)) +c_AuxiliaryConsoleShow = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_ubyte, ct.c_int32)(("simxAuxiliaryConsoleShow", libsimx)) +c_GetObjectOrientation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectOrientation", libsimx)) +c_GetObjectQuaternion = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectQuaternion", libsimx)) +c_GetObjectPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectPosition", libsimx)) +c_SetObjectOrientation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetObjectOrientation", libsimx)) +c_SetObjectQuaternion = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetObjectQuaternion", libsimx)) +c_SetObjectPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetObjectPosition", libsimx)) +c_SetObjectParent = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_ubyte, ct.c_int32)(("simxSetObjectParent", libsimx)) +c_SetUIButtonLabel = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_char), ct.c_int32)(("simxSetUIButtonLabel", libsimx)) +c_GetLastErrors = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_char)), ct.c_int32)(("simxGetLastErrors", libsimx)) +c_GetArrayParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetArrayParameter", libsimx)) +c_SetArrayParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetArrayParameter", libsimx)) +c_GetBooleanParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.c_int32)(("simxGetBooleanParameter", libsimx)) +c_SetBooleanParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_ubyte, ct.c_int32)(("simxSetBooleanParameter", libsimx)) +c_GetIntegerParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetIntegerParameter", libsimx)) +c_SetIntegerParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetIntegerParameter", libsimx)) +c_GetFloatingParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetFloatingParameter", libsimx)) +c_SetFloatingParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetFloatingParameter", libsimx)) +c_GetStringParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.POINTER(ct.c_char)), ct.c_int32)(("simxGetStringParameter", libsimx)) +c_GetCollisionHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetCollisionHandle", libsimx)) +c_GetDistanceHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetDistanceHandle", libsimx)) +c_GetCollectionHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetCollectionHandle", libsimx)) +c_ReadCollision = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.c_int32)(("simxReadCollision", libsimx)) +c_ReadDistance = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxReadDistance", libsimx)) +c_RemoveObject = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxRemoveObject", libsimx)) +c_RemoveModel = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxRemoveModel", libsimx)) +c_RemoveUI = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxRemoveUI", libsimx)) +c_CloseScene = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32)(("simxCloseScene", libsimx)) +c_GetObjects = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)), ct.c_int32)(("simxGetObjects", libsimx)) +c_DisplayDialog = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_char), ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_float), ct.POINTER(ct.c_float), ct.POINTER(ct.c_int32), ct.POINTER(ct.c_int32), ct.c_int32)(("simxDisplayDialog", libsimx)) +c_EndDialog = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxEndDialog", libsimx)) +c_GetDialogInput = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.POINTER(ct.c_char)), ct.c_int32)(("simxGetDialogInput", libsimx)) +c_GetDialogResult = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetDialogResult", libsimx)) +c_CopyPasteObjects = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32, ct.POINTER(ct.POINTER(ct.c_int32)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxCopyPasteObjects", libsimx)) +c_GetObjectSelection = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.POINTER(ct.c_int32)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectSelection", libsimx)) +c_SetObjectSelection = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32, ct.c_int32)(("simxSetObjectSelection", libsimx)) +c_ClearFloatSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxClearFloatSignal", libsimx)) +c_ClearIntegerSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxClearIntegerSignal", libsimx)) +c_ClearStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxClearStringSignal", libsimx)) +c_GetFloatSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_float), ct.c_int32)(("simxGetFloatSignal", libsimx)) +c_GetIntegerSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetIntegerSignal", libsimx)) +c_GetStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.POINTER(ct.c_ubyte)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetStringSignal", libsimx)) +c_SetFloatSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_float, ct.c_int32)(("simxSetFloatSignal", libsimx)) +c_SetIntegerSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32, ct.c_int32)(("simxSetIntegerSignal", libsimx)) +c_SetStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_ubyte), ct.c_int32, ct.c_int32)(("simxSetStringSignal", libsimx)) +c_AppendStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_ubyte), ct.c_int32, ct.c_int32)(("simxAppendStringSignal", libsimx)) +c_WriteStringStream = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_ubyte), ct.c_int32, ct.c_int32)(("simxWriteStringStream", libsimx)) +c_GetObjectFloatParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectFloatParameter", libsimx)) +c_SetObjectFloatParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetObjectFloatParameter", libsimx)) +c_GetObjectIntParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectIntParameter", libsimx)) +c_SetObjectIntParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetObjectIntParameter", libsimx)) +c_GetModelProperty = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetModelProperty", libsimx)) +c_SetModelProperty = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetModelProperty", libsimx)) +c_Start = ct.CFUNCTYPE(ct.c_int32,ct.POINTER(ct.c_char), ct.c_int32, ct.c_ubyte, ct.c_ubyte, ct.c_int32, ct.c_int32)(("simxStart", libsimx)) +c_Finish = ct.CFUNCTYPE(None, ct.c_int32)(("simxFinish", libsimx)) +c_GetPingTime = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_int32))(("simxGetPingTime", libsimx)) +c_GetLastCmdTime = ct.CFUNCTYPE(ct.c_int32,ct.c_int32)(("simxGetLastCmdTime", libsimx)) +c_SynchronousTrigger = ct.CFUNCTYPE(ct.c_int32,ct.c_int32)(("simxSynchronousTrigger", libsimx)) +c_Synchronous = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_ubyte)(("simxSynchronous", libsimx)) +c_PauseCommunication = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_ubyte)(("simxPauseCommunication", libsimx)) +c_GetInMessageInfo = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32))(("simxGetInMessageInfo", libsimx)) +c_GetOutMessageInfo = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32))(("simxGetOutMessageInfo", libsimx)) +c_GetConnectionId = ct.CFUNCTYPE(ct.c_int32,ct.c_int32)(("simxGetConnectionId", libsimx)) +c_CreateBuffer = ct.CFUNCTYPE(ct.POINTER(ct.c_ubyte), ct.c_int32)(("simxCreateBuffer", libsimx)) +c_ReleaseBuffer = ct.CFUNCTYPE(None, ct.c_void_p)(("simxReleaseBuffer", libsimx)) +c_TransferFile = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_char), ct.c_int32, ct.c_int32)(("simxTransferFile", libsimx)) +c_EraseFile = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxEraseFile", libsimx)) +c_GetAndClearStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.POINTER(ct.c_ubyte)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetAndClearStringSignal", libsimx)) +c_ReadStringStream = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.POINTER(ct.c_ubyte)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxReadStringStream", libsimx)) +c_CreateDummy = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_float, ct.POINTER(ct.c_ubyte), ct.POINTER(ct.c_int32), ct.c_int32)(("simxCreateDummy", libsimx)) +c_Query = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_ubyte), ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.POINTER(ct.c_ubyte)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxQuery", libsimx)) +c_GetObjectGroupData = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)), ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)), ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_float)), ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_char)), ct.c_int32)(("simxGetObjectGroupData", libsimx)) +c_GetObjectVelocity = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectVelocity", libsimx)) +c_CallScriptFunction = ct.CFUNCTYPE(ct.c_int32,ct.c_int32,ct.POINTER(ct.c_char),ct.c_int32,ct.POINTER(ct.c_char),ct.c_int32,ct.POINTER(ct.c_int32),ct.c_int32,ct.POINTER(ct.c_float),ct.c_int32,ct.POINTER(ct.c_char),ct.c_int32,ct.POINTER(ct.c_ubyte),ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)),ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_float)),ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_char)),ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_ubyte)),ct.c_int32)(("simxCallScriptFunction", libsimx)) + +#API functions +def simxGetJointPosition(clientID, jointHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + position = ct.c_float() + return c_GetJointPosition(clientID, jointHandle, ct.byref(position), operationMode), position.value + +def simxSetJointPosition(clientID, jointHandle, position, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_SetJointPosition(clientID, jointHandle, position, operationMode) + +def simxGetJointMatrix(clientID, jointHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + matrix = (ct.c_float*12)() + ret = c_GetJointMatrix(clientID, jointHandle, matrix, operationMode) + arr = [] + for i in range(12): + arr.append(matrix[i]) + return ret, arr + +def simxSetSphericalJointMatrix(clientID, jointHandle, matrix, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + matrix = (ct.c_float*12)(*matrix) + return c_SetSphericalJointMatrix(clientID, jointHandle, matrix, operationMode) + +def simxSetJointTargetVelocity(clientID, jointHandle, targetVelocity, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_SetJointTargetVelocity(clientID, jointHandle, targetVelocity, operationMode) + +def simxSetJointTargetPosition(clientID, jointHandle, targetPosition, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_SetJointTargetPosition(clientID, jointHandle, targetPosition, operationMode) + +def simxJointGetForce(clientID, jointHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + force = ct.c_float() + return c_GetJointForce(clientID, jointHandle, ct.byref(force), operationMode), force.value + +def simxGetJointForce(clientID, jointHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + force = ct.c_float() + return c_GetJointForce(clientID, jointHandle, ct.byref(force), operationMode), force.value + +def simxGetJointMaxForce(clientID, jointHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + force = ct.c_float() + return c_GetJointMaxForce(clientID, jointHandle, ct.byref(force), operationMode), force.value + +def simxSetJointForce(clientID, jointHandle, force, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + return c_SetJointMaxForce(clientID, jointHandle, force, operationMode) + +def simxSetJointMaxForce(clientID, jointHandle, force, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + return c_SetJointMaxForce(clientID, jointHandle, force, operationMode) + +def simxReadForceSensor(clientID, forceSensorHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + state = ct.c_ubyte() + forceVector = (ct.c_float*3)() + torqueVector = (ct.c_float*3)() + ret = c_ReadForceSensor(clientID, forceSensorHandle, ct.byref(state), forceVector, torqueVector, operationMode) + arr1 = [] + for i in range(3): + arr1.append(forceVector[i]) + arr2 = [] + for i in range(3): + arr2.append(torqueVector[i]) + #if sys.version_info[0] == 3: + # state=state.value + #else: + # state=ord(state.value) + return ret, state.value, arr1, arr2 + +def simxBreakForceSensor(clientID, forceSensorHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + return c_BreakForceSensor(clientID, forceSensorHandle, operationMode) + +def simxReadVisionSensor(clientID, sensorHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + detectionState = ct.c_ubyte() + auxValues = ct.POINTER(ct.c_float)() + auxValuesCount = ct.POINTER(ct.c_int)() + ret = c_ReadVisionSensor(clientID, sensorHandle, ct.byref(detectionState), ct.byref(auxValues), ct.byref(auxValuesCount), operationMode) + + auxValues2 = [] + if ret == 0: + s = 0 + for i in range(auxValuesCount[0]): + auxValues2.append(auxValues[s:s+auxValuesCount[i+1]]) + s += auxValuesCount[i+1] + + #free C buffers + c_ReleaseBuffer(auxValues) + c_ReleaseBuffer(auxValuesCount) + + return ret, bool(detectionState.value!=0), auxValues2 + +def simxGetObjectHandle(clientID, objectName, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + handle = ct.c_int() + if (sys.version_info[0] == 3) and (type(objectName) is str): + objectName=objectName.encode('utf-8') + return c_GetObjectHandle(clientID, objectName, ct.byref(handle), operationMode), handle.value + +def simxGetVisionSensorImage(clientID, sensorHandle, options, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + resolution = (ct.c_int*2)() + c_image = ct.POINTER(ct.c_byte)() + bytesPerPixel = 3 + if (options and 1) != 0: + bytesPerPixel = 1 + ret = c_GetVisionSensorImage(clientID, sensorHandle, resolution, ct.byref(c_image), options, operationMode) + + reso = [] + image = [] + if (ret == 0): + image = [None]*resolution[0]*resolution[1]*bytesPerPixel + for i in range(resolution[0] * resolution[1] * bytesPerPixel): + image[i] = c_image[i] + for i in range(2): + reso.append(resolution[i]) + return ret, reso, image + +def simxSetVisionSensorImage(clientID, sensorHandle, image, options, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + size = len(image) + image_bytes = (ct.c_byte*size)(*image) + return c_SetVisionSensorImage(clientID, sensorHandle, image_bytes, size, options, operationMode) + +def simxGetVisionSensorDepthBuffer(clientID, sensorHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + c_buffer = ct.POINTER(ct.c_float)() + resolution = (ct.c_int*2)() + ret = c_GetVisionSensorDepthBuffer(clientID, sensorHandle, resolution, ct.byref(c_buffer), operationMode) + reso = [] + buffer = [] + if (ret == 0): + buffer = [None]*resolution[0]*resolution[1] + for i in range(resolution[0] * resolution[1]): + buffer[i] = c_buffer[i] + for i in range(2): + reso.append(resolution[i]) + return ret, reso, buffer + +def simxGetObjectChild(clientID, parentObjectHandle, childIndex, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + childObjectHandle = ct.c_int() + return c_GetObjectChild(clientID, parentObjectHandle, childIndex, ct.byref(childObjectHandle), operationMode), childObjectHandle.value + +def simxGetObjectParent(clientID, childObjectHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + parentObjectHandle = ct.c_int() + return c_GetObjectParent(clientID, childObjectHandle, ct.byref(parentObjectHandle), operationMode), parentObjectHandle.value + +def simxReadProximitySensor(clientID, sensorHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + detectionState = ct.c_ubyte() + detectedObjectHandle = ct.c_int() + detectedPoint = (ct.c_float*3)() + detectedSurfaceNormalVector = (ct.c_float*3)() + ret = c_ReadProximitySensor(clientID, sensorHandle, ct.byref(detectionState), detectedPoint, ct.byref(detectedObjectHandle), detectedSurfaceNormalVector, operationMode) + arr1 = [] + for i in range(3): + arr1.append(detectedPoint[i]) + arr2 = [] + for i in range(3): + arr2.append(detectedSurfaceNormalVector[i]) + return ret, bool(detectionState.value!=0), arr1, detectedObjectHandle.value, arr2 + +def simxLoadModel(clientID, modelPathAndName, options, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + baseHandle = ct.c_int() + if (sys.version_info[0] == 3) and (type(modelPathAndName) is str): + modelPathAndName=modelPathAndName.encode('utf-8') + return c_LoadModel(clientID, modelPathAndName, options, ct.byref(baseHandle), operationMode), baseHandle.value + +def simxLoadUI(clientID, uiPathAndName, options, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + count = ct.c_int() + uiHandles = ct.POINTER(ct.c_int)() + if (sys.version_info[0] == 3) and (type(uiPathAndName) is str): + uiPathAndName=uiPathAndName.encode('utf-8') + ret = c_LoadUI(clientID, uiPathAndName, options, ct.byref(count), ct.byref(uiHandles), operationMode) + + handles = [] + if ret == 0: + for i in range(count.value): + handles.append(uiHandles[i]) + #free C buffers + c_ReleaseBuffer(uiHandles) + + return ret, handles + +def simxLoadScene(clientID, scenePathAndName, options, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + if (sys.version_info[0] == 3) and (type(scenePathAndName) is str): + scenePathAndName=scenePathAndName.encode('utf-8') + return c_LoadScene(clientID, scenePathAndName, options, operationMode) + +def simxStartSimulation(clientID, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_StartSimulation(clientID, operationMode) + +def simxPauseSimulation(clientID, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_PauseSimulation(clientID, operationMode) + +def simxStopSimulation(clientID, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_StopSimulation(clientID, operationMode) + +def simxGetUIHandle(clientID, uiName, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + handle = ct.c_int() + if (sys.version_info[0] == 3) and (type(uiName) is str): + uiName=uiName.encode('utf-8') + return c_GetUIHandle(clientID, uiName, ct.byref(handle), operationMode), handle.value + +def simxGetUISlider(clientID, uiHandle, uiButtonID, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + position = ct.c_int() + return c_GetUISlider(clientID, uiHandle, uiButtonID, ct.byref(position), operationMode), position.value + +def simxSetUISlider(clientID, uiHandle, uiButtonID, position, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_SetUISlider(clientID, uiHandle, uiButtonID, position, operationMode) + +def simxGetUIEventButton(clientID, uiHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + uiEventButtonID = ct.c_int() + auxValues = (ct.c_int*2)() + ret = c_GetUIEventButton(clientID, uiHandle, ct.byref(uiEventButtonID), auxValues, operationMode) + arr = [] + for i in range(2): + arr.append(auxValues[i]) + return ret, uiEventButtonID.value, arr + +def simxGetUIButtonProperty(clientID, uiHandle, uiButtonID, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + prop = ct.c_int() + return c_GetUIButtonProperty(clientID, uiHandle, uiButtonID, ct.byref(prop), operationMode), prop.value + +def simxSetUIButtonProperty(clientID, uiHandle, uiButtonID, prop, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_SetUIButtonProperty(clientID, uiHandle, uiButtonID, prop, operationMode) + +def simxAddStatusbarMessage(clientID, message, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + if (sys.version_info[0] == 3) and (type(message) is str): + message=message.encode('utf-8') + return c_AddStatusbarMessage(clientID, message, operationMode) + +def simxAuxiliaryConsoleOpen(clientID, title, maxLines, mode, position, size, textColor, backgroundColor, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + consoleHandle = ct.c_int() + if (sys.version_info[0] == 3) and (type(title) is str): + title=title.encode('utf-8') + if position != None: + c_position = (ct.c_int*2)(*position) + else: + c_position = None + if size != None: + c_size = (ct.c_int*2)(*size) + else: + c_size = None + if textColor != None: + c_textColor = (ct.c_float*3)(*textColor) + else: + c_textColor = None + if backgroundColor != None: + c_backgroundColor = (ct.c_float*3)(*backgroundColor) + else: + c_backgroundColor = None + return c_AuxiliaryConsoleOpen(clientID, title, maxLines, mode, c_position, c_size, c_textColor, c_backgroundColor, ct.byref(consoleHandle), operationMode), consoleHandle.value + +def simxAuxiliaryConsoleClose(clientID, consoleHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_AuxiliaryConsoleClose(clientID, consoleHandle, operationMode) + +def simxAuxiliaryConsolePrint(clientID, consoleHandle, txt, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + if (sys.version_info[0] == 3) and (type(txt) is str): + txt=txt.encode('utf-8') + return c_AuxiliaryConsolePrint(clientID, consoleHandle, txt, operationMode) + +def simxAuxiliaryConsoleShow(clientID, consoleHandle, showState, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_AuxiliaryConsoleShow(clientID, consoleHandle, showState, operationMode) + +def simxGetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + eulerAngles = (ct.c_float*3)() + ret = c_GetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, eulerAngles, operationMode) + arr = [] + for i in range(3): + arr.append(eulerAngles[i]) + return ret, arr + +def simxGetObjectQuaternion(clientID, objectHandle, relativeToObjectHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + quaternion = (ct.c_float*4)() + ret = c_GetObjectQuaternion(clientID, objectHandle, relativeToObjectHandle, quaternion, operationMode) + arr = [] + for i in range(4): + arr.append(quaternion[i]) + return ret, arr + +def simxGetObjectPosition(clientID, objectHandle, relativeToObjectHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + position = (ct.c_float*3)() + ret = c_GetObjectPosition(clientID, objectHandle, relativeToObjectHandle, position, operationMode) + arr = [] + for i in range(3): + arr.append(position[i]) + return ret, arr + +def simxSetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, eulerAngles, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + angles = (ct.c_float*3)(*eulerAngles) + return c_SetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, angles, operationMode) + +def simxSetObjectQuaternion(clientID, objectHandle, relativeToObjectHandle, quaternion, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + quat = (ct.c_float*4)(*quaternion) + return c_SetObjectQuaternion(clientID, objectHandle, relativeToObjectHandle, quat, operationMode) + +def simxSetObjectPosition(clientID, objectHandle, relativeToObjectHandle, position, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + c_position = (ct.c_float*3)(*position) + return c_SetObjectPosition(clientID, objectHandle, relativeToObjectHandle, c_position, operationMode) + +def simxSetObjectParent(clientID, objectHandle, parentObject, keepInPlace, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_SetObjectParent(clientID, objectHandle, parentObject, keepInPlace, operationMode) + +def simxSetUIButtonLabel(clientID, uiHandle, uiButtonID, upStateLabel, downStateLabel, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + if sys.version_info[0] == 3: + if type(upStateLabel) is str: + upStateLabel=upStateLabel.encode('utf-8') + if type(downStateLabel) is str: + downStateLabel=downStateLabel.encode('utf-8') + return c_SetUIButtonLabel(clientID, uiHandle, uiButtonID, upStateLabel, downStateLabel, operationMode) + +def simxGetLastErrors(clientID, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + errors =[] + errorCnt = ct.c_int() + errorStrings = ct.POINTER(ct.c_char)() + ret = c_GetLastErrors(clientID, ct.byref(errorCnt), ct.byref(errorStrings), operationMode) + if ret == 0: + s = 0 + for i in range(errorCnt.value): + a = bytearray() + while errorStrings[s] != b'\0': + if sys.version_info[0] == 3: + a.append(int.from_bytes(errorStrings[s],'big')) + else: + a.append(errorStrings[s]) + s += 1 + s += 1 #skip null + if sys.version_info[0] == 3: + errors.append(str(a,'utf-8')) + else: + errors.append(str(a)) + + return ret, errors + +def simxGetArrayParameter(clientID, paramIdentifier, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + paramValues = (ct.c_float*3)() + ret = c_GetArrayParameter(clientID, paramIdentifier, paramValues, operationMode) + arr = [] + for i in range(3): + arr.append(paramValues[i]) + return ret, arr + +def simxSetArrayParameter(clientID, paramIdentifier, paramValues, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + c_paramValues = (ct.c_float*3)(*paramValues) + return c_SetArrayParameter(clientID, paramIdentifier, c_paramValues, operationMode) + +def simxGetBooleanParameter(clientID, paramIdentifier, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + paramValue = ct.c_ubyte() + return c_GetBooleanParameter(clientID, paramIdentifier, ct.byref(paramValue), operationMode), bool(paramValue.value!=0) + +def simxSetBooleanParameter(clientID, paramIdentifier, paramValue, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_SetBooleanParameter(clientID, paramIdentifier, paramValue, operationMode) + +def simxGetIntegerParameter(clientID, paramIdentifier, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + paramValue = ct.c_int() + return c_GetIntegerParameter(clientID, paramIdentifier, ct.byref(paramValue), operationMode), paramValue.value + +def simxSetIntegerParameter(clientID, paramIdentifier, paramValue, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_SetIntegerParameter(clientID, paramIdentifier, paramValue, operationMode) + +def simxGetFloatingParameter(clientID, paramIdentifier, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + paramValue = ct.c_float() + return c_GetFloatingParameter(clientID, paramIdentifier, ct.byref(paramValue), operationMode), paramValue.value + +def simxSetFloatingParameter(clientID, paramIdentifier, paramValue, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_SetFloatingParameter(clientID, paramIdentifier, paramValue, operationMode) + +def simxGetStringParameter(clientID, paramIdentifier, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + paramValue = ct.POINTER(ct.c_char)() + ret = c_GetStringParameter(clientID, paramIdentifier, ct.byref(paramValue), operationMode) + + a = bytearray() + if ret == 0: + i = 0 + while paramValue[i] != b'\0': + if sys.version_info[0] == 3: + a.append(int.from_bytes(paramValue[i],'big')) + else: + a.append(paramValue[i]) + i=i+1 + if sys.version_info[0] == 3: + a=str(a,'utf-8') + else: + a=str(a) + return ret, a + +def simxGetCollisionHandle(clientID, collisionObjectName, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + handle = ct.c_int() + if (sys.version_info[0] == 3) and (type(collisionObjectName) is str): + collisionObjectName=collisionObjectName.encode('utf-8') + return c_GetCollisionHandle(clientID, collisionObjectName, ct.byref(handle), operationMode), handle.value + +def simxGetCollectionHandle(clientID, collectionName, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + handle = ct.c_int() + if (sys.version_info[0] == 3) and (type(collectionName) is str): + collectionName=collectionName.encode('utf-8') + return c_GetCollectionHandle(clientID, collectionName, ct.byref(handle), operationMode), handle.value + +def simxGetDistanceHandle(clientID, distanceObjectName, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + handle = ct.c_int() + if (sys.version_info[0] == 3) and (type(distanceObjectName) is str): + distanceObjectName=distanceObjectName.encode('utf-8') + return c_GetDistanceHandle(clientID, distanceObjectName, ct.byref(handle), operationMode), handle.value + +def simxReadCollision(clientID, collisionObjectHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + collisionState = ct.c_ubyte() + return c_ReadCollision(clientID, collisionObjectHandle, ct.byref(collisionState), operationMode), bool(collisionState.value!=0) + +def simxReadDistance(clientID, distanceObjectHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + minimumDistance = ct.c_float() + return c_ReadDistance(clientID, distanceObjectHandle, ct.byref(minimumDistance), operationMode), minimumDistance.value + +def simxRemoveObject(clientID, objectHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_RemoveObject(clientID, objectHandle, operationMode) + +def simxRemoveModel(clientID, objectHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_RemoveModel(clientID, objectHandle, operationMode) + +def simxRemoveUI(clientID, uiHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_RemoveUI(clientID, uiHandle, operationMode) + +def simxCloseScene(clientID, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_CloseScene(clientID, operationMode) + +def simxGetObjects(clientID, objectType, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + objectCount = ct.c_int() + objectHandles = ct.POINTER(ct.c_int)() + + ret = c_GetObjects(clientID, objectType, ct.byref(objectCount), ct.byref(objectHandles), operationMode) + handles = [] + if ret == 0: + for i in range(objectCount.value): + handles.append(objectHandles[i]) + + return ret, handles + + +def simxDisplayDialog(clientID, titleText, mainText, dialogType, initialText, titleColors, dialogColors, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + if titleColors != None: + c_titleColors = (ct.c_float*6)(*titleColors) + else: + c_titleColors = None + if dialogColors != None: + c_dialogColors = (ct.c_float*6)(*dialogColors) + else: + c_dialogColors = None + + c_dialogHandle = ct.c_int() + c_uiHandle = ct.c_int() + if sys.version_info[0] == 3: + if type(titleText) is str: + titleText=titleText.encode('utf-8') + if type(mainText) is str: + mainText=mainText.encode('utf-8') + if type(initialText) is str: + initialText=initialText.encode('utf-8') + return c_DisplayDialog(clientID, titleText, mainText, dialogType, initialText, c_titleColors, c_dialogColors, ct.byref(c_dialogHandle), ct.byref(c_uiHandle), operationMode), c_dialogHandle.value, c_uiHandle.value + +def simxEndDialog(clientID, dialogHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_EndDialog(clientID, dialogHandle, operationMode) + +def simxGetDialogInput(clientID, dialogHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + inputText = ct.POINTER(ct.c_char)() + ret = c_GetDialogInput(clientID, dialogHandle, ct.byref(inputText), operationMode) + + a = bytearray() + if ret == 0: + i = 0 + while inputText[i] != b'\0': + if sys.version_info[0] == 3: + a.append(int.from_bytes(inputText[i],'big')) + else: + a.append(inputText[i]) + i = i+1 + + if sys.version_info[0] == 3: + a=str(a,'utf-8') + else: + a=str(a) + return ret, a + + +def simxGetDialogResult(clientID, dialogHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + result = ct.c_int() + return c_GetDialogResult(clientID, dialogHandle, ct.byref(result), operationMode), result.value + +def simxCopyPasteObjects(clientID, objectHandles, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + c_objectHandles = (ct.c_int*len(objectHandles))(*objectHandles) + c_objectHandles = ct.cast(c_objectHandles,ct.POINTER(ct.c_int)) # IronPython needs this + newObjectCount = ct.c_int() + newObjectHandles = ct.POINTER(ct.c_int)() + ret = c_CopyPasteObjects(clientID, c_objectHandles, len(objectHandles), ct.byref(newObjectHandles), ct.byref(newObjectCount), operationMode) + + newobj = [] + if ret == 0: + for i in range(newObjectCount.value): + newobj.append(newObjectHandles[i]) + + return ret, newobj + + +def simxGetObjectSelection(clientID, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + objectCount = ct.c_int() + objectHandles = ct.POINTER(ct.c_int)() + ret = c_GetObjectSelection(clientID, ct.byref(objectHandles), ct.byref(objectCount), operationMode) + + newobj = [] + if ret == 0: + for i in range(objectCount.value): + newobj.append(objectHandles[i]) + + return ret, newobj + + + +def simxSetObjectSelection(clientID, objectHandles, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + c_objectHandles = (ct.c_int*len(objectHandles))(*objectHandles) + return c_SetObjectSelection(clientID, c_objectHandles, len(objectHandles), operationMode) + +def simxClearFloatSignal(clientID, signalName, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + if (sys.version_info[0] == 3) and (type(signalName) is str): + signalName=signalName.encode('utf-8') + return c_ClearFloatSignal(clientID, signalName, operationMode) + +def simxClearIntegerSignal(clientID, signalName, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + if (sys.version_info[0] == 3) and (type(signalName) is str): + signalName=signalName.encode('utf-8') + return c_ClearIntegerSignal(clientID, signalName, operationMode) + +def simxClearStringSignal(clientID, signalName, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + if (sys.version_info[0] == 3) and (type(signalName) is str): + signalName=signalName.encode('utf-8') + return c_ClearStringSignal(clientID, signalName, operationMode) + +def simxGetFloatSignal(clientID, signalName, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + signalValue = ct.c_float() + if (sys.version_info[0] == 3) and (type(signalName) is str): + signalName=signalName.encode('utf-8') + return c_GetFloatSignal(clientID, signalName, ct.byref(signalValue), operationMode), signalValue.value + +def simxGetIntegerSignal(clientID, signalName, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + signalValue = ct.c_int() + if (sys.version_info[0] == 3) and (type(signalName) is str): + signalName=signalName.encode('utf-8') + return c_GetIntegerSignal(clientID, signalName, ct.byref(signalValue), operationMode), signalValue.value + +def simxGetStringSignal(clientID, signalName, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + signalLength = ct.c_int(); + signalValue = ct.POINTER(ct.c_ubyte)() + if (sys.version_info[0] == 3) and (type(signalName) is str): + signalName=signalName.encode('utf-8') + ret = c_GetStringSignal(clientID, signalName, ct.byref(signalValue), ct.byref(signalLength), operationMode) + + a = bytearray() + if ret == 0: + for i in range(signalLength.value): + a.append(signalValue[i]) + if sys.version_info[0] != 3: + a=str(a) + + return ret, a + +def simxGetAndClearStringSignal(clientID, signalName, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + signalLength = ct.c_int(); + signalValue = ct.POINTER(ct.c_ubyte)() + if (sys.version_info[0] == 3) and (type(signalName) is str): + signalName=signalName.encode('utf-8') + ret = c_GetAndClearStringSignal(clientID, signalName, ct.byref(signalValue), ct.byref(signalLength), operationMode) + + a = bytearray() + if ret == 0: + for i in range(signalLength.value): + a.append(signalValue[i]) + if sys.version_info[0] != 3: + a=str(a) + + return ret, a + +def simxReadStringStream(clientID, signalName, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + signalLength = ct.c_int(); + signalValue = ct.POINTER(ct.c_ubyte)() + if (sys.version_info[0] == 3) and (type(signalName) is str): + signalName=signalName.encode('utf-8') + ret = c_ReadStringStream(clientID, signalName, ct.byref(signalValue), ct.byref(signalLength), operationMode) + + a = bytearray() + if ret == 0: + for i in range(signalLength.value): + a.append(signalValue[i]) + if sys.version_info[0] != 3: + a=str(a) + + return ret, a + +def simxSetFloatSignal(clientID, signalName, signalValue, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + if (sys.version_info[0] == 3) and (type(signalName) is str): + signalName=signalName.encode('utf-8') + return c_SetFloatSignal(clientID, signalName, signalValue, operationMode) + +def simxSetIntegerSignal(clientID, signalName, signalValue, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + if (sys.version_info[0] == 3) and (type(signalName) is str): + signalName=signalName.encode('utf-8') + return c_SetIntegerSignal(clientID, signalName, signalValue, operationMode) + +def simxSetStringSignal(clientID, signalName, signalValue, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + sigV=signalValue + if sys.version_info[0] == 3: + if type(signalName) is str: + signalName=signalName.encode('utf-8') + if type(signalValue) is bytearray: + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + if type(signalValue) is str: + signalValue=signalValue.encode('utf-8') + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + else: + if type(signalValue) is bytearray: + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + if type(signalValue) is str: + signalValue=bytearray(signalValue) + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + sigV=ct.cast(sigV,ct.POINTER(ct.c_ubyte)) # IronPython needs this + return c_SetStringSignal(clientID, signalName, sigV, len(signalValue), operationMode) + +def simxAppendStringSignal(clientID, signalName, signalValue, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + sigV=signalValue + if sys.version_info[0] == 3: + if type(signalName) is str: + signalName=signalName.encode('utf-8') + if type(signalValue) is bytearray: + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + if type(signalValue) is str: + signalValue=signalValue.encode('utf-8') + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + else: + if type(signalValue) is bytearray: + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + if type(signalValue) is str: + signalValue=bytearray(signalValue) + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + sigV=ct.cast(sigV,ct.POINTER(ct.c_ubyte)) # IronPython needs this + return c_AppendStringSignal(clientID, signalName, sigV, len(signalValue), operationMode) + +def simxWriteStringStream(clientID, signalName, signalValue, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + sigV=signalValue + if sys.version_info[0] == 3: + if type(signalName) is str: + signalName=signalName.encode('utf-8') + if type(signalValue) is bytearray: + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + if type(signalValue) is str: + signalValue=signalValue.encode('utf-8') + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + else: + if type(signalValue) is bytearray: + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + if type(signalValue) is str: + signalValue=bytearray(signalValue) + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + sigV=ct.cast(sigV,ct.POINTER(ct.c_ubyte)) # IronPython needs this + return c_WriteStringStream(clientID, signalName, sigV, len(signalValue), operationMode) + +def simxGetObjectFloatParameter(clientID, objectHandle, parameterID, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + parameterValue = ct.c_float() + return c_GetObjectFloatParameter(clientID, objectHandle, parameterID, ct.byref(parameterValue), operationMode), parameterValue.value + +def simxSetObjectFloatParameter(clientID, objectHandle, parameterID, parameterValue, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_SetObjectFloatParameter(clientID, objectHandle, parameterID, parameterValue, operationMode) + +def simxGetObjectIntParameter(clientID, objectHandle, parameterID, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + parameterValue = ct.c_int() + return c_GetObjectIntParameter(clientID, objectHandle, parameterID, ct.byref(parameterValue), operationMode), parameterValue.value + +def simxSetObjectIntParameter(clientID, objectHandle, parameterID, parameterValue, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_SetObjectIntParameter(clientID, objectHandle, parameterID, parameterValue, operationMode) + +def simxGetModelProperty(clientID, objectHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + prop = ct.c_int() + return c_GetModelProperty(clientID, objectHandle, ct.byref(prop), operationMode), prop.value + +def simxSetModelProperty(clientID, objectHandle, prop, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_SetModelProperty(clientID, objectHandle, prop, operationMode) + +def simxStart(connectionAddress, connectionPort, waitUntilConnected, doNotReconnectOnceDisconnected, timeOutInMs, commThreadCycleInMs): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + if (sys.version_info[0] == 3) and (type(connectionAddress) is str): + connectionAddress=connectionAddress.encode('utf-8') + return c_Start(connectionAddress, connectionPort, waitUntilConnected, doNotReconnectOnceDisconnected, timeOutInMs, commThreadCycleInMs) + +def simxFinish(clientID): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_Finish(clientID) + +def simxGetPingTime(clientID): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + pingTime = ct.c_int() + return c_GetPingTime(clientID, ct.byref(pingTime)), pingTime.value + +def simxGetLastCmdTime(clientID): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_GetLastCmdTime(clientID) + +def simxSynchronousTrigger(clientID): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_SynchronousTrigger(clientID) + +def simxSynchronous(clientID, enable): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_Synchronous(clientID, enable) + +def simxPauseCommunication(clientID, enable): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_PauseCommunication(clientID, enable) + +def simxGetInMessageInfo(clientID, infoType): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + info = ct.c_int() + return c_GetInMessageInfo(clientID, infoType, ct.byref(info)), info.value + +def simxGetOutMessageInfo(clientID, infoType): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + info = ct.c_int() + return c_GetOutMessageInfo(clientID, infoType, ct.byref(info)), info.value + +def simxGetConnectionId(clientID): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_GetConnectionId(clientID) + +def simxCreateBuffer(bufferSize): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_CreateBuffer(bufferSize) + +def simxReleaseBuffer(buffer): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + return c_ReleaseBuffer(buffer) + +def simxTransferFile(clientID, filePathAndName, fileName_serverSide, timeOut, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + if (sys.version_info[0] == 3) and (type(filePathAndName) is str): + filePathAndName=filePathAndName.encode('utf-8') + return c_TransferFile(clientID, filePathAndName, fileName_serverSide, timeOut, operationMode) + +def simxEraseFile(clientID, fileName_serverSide, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + if (sys.version_info[0] == 3) and (type(fileName_serverSide) is str): + fileName_serverSide=fileName_serverSide.encode('utf-8') + return c_EraseFile(clientID, fileName_serverSide, operationMode) + +def simxCreateDummy(clientID, size, color, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + handle = ct.c_int() + if color != None: + c_color = (ct.c_ubyte*12)(*color) + else: + c_color = None + return c_CreateDummy(clientID, size, c_color, ct.byref(handle), operationMode), handle.value + +def simxQuery(clientID, signalName, signalValue, retSignalName, timeOutInMs): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + retSignalLength = ct.c_int(); + retSignalValue = ct.POINTER(ct.c_ubyte)() + + sigV=signalValue + if sys.version_info[0] == 3: + if type(signalName) is str: + signalName=signalName.encode('utf-8') + if type(retSignalName) is str: + retSignalName=retSignalName.encode('utf-8') + if type(signalValue) is bytearray: + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + if type(signalValue) is str: + signalValue=signalValue.encode('utf-8') + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + else: + if type(signalValue) is bytearray: + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + if type(signalValue) is str: + signalValue=bytearray(signalValue) + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + sigV=ct.cast(sigV,ct.POINTER(ct.c_ubyte)) # IronPython needs this + + ret = c_Query(clientID, signalName, sigV, len(signalValue), retSignalName, ct.byref(retSignalValue), ct.byref(retSignalLength), timeOutInMs) + + a = bytearray() + if ret == 0: + for i in range(retSignalLength.value): + a.append(retSignalValue[i]) + if sys.version_info[0] != 3: + a=str(a) + + return ret, a + +def simxGetObjectGroupData(clientID, objectType, dataType, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + handles =[] + intData =[] + floatData =[] + stringData =[] + handlesC = ct.c_int() + handlesP = ct.POINTER(ct.c_int)() + intDataC = ct.c_int() + intDataP = ct.POINTER(ct.c_int)() + floatDataC = ct.c_int() + floatDataP = ct.POINTER(ct.c_float)() + stringDataC = ct.c_int() + stringDataP = ct.POINTER(ct.c_char)() + ret = c_GetObjectGroupData(clientID, objectType, dataType, ct.byref(handlesC), ct.byref(handlesP), ct.byref(intDataC), ct.byref(intDataP), ct.byref(floatDataC), ct.byref(floatDataP), ct.byref(stringDataC), ct.byref(stringDataP), operationMode) + + if ret == 0: + for i in range(handlesC.value): + handles.append(handlesP[i]) + for i in range(intDataC.value): + intData.append(intDataP[i]) + for i in range(floatDataC.value): + floatData.append(floatDataP[i]) + s = 0 + for i in range(stringDataC.value): + a = bytearray() + while stringDataP[s] != b'\0': + if sys.version_info[0] == 3: + a.append(int.from_bytes(stringDataP[s],'big')) + else: + a.append(stringDataP[s]) + s += 1 + s += 1 #skip null + if sys.version_info[0] == 3: + a=str(a,'utf-8') + else: + a=str(a) + stringData.append(a) + + return ret, handles, intData, floatData, stringData + +def simxCallScriptFunction(clientID, scriptDescription, options, functionName, inputInts, inputFloats, inputStrings, inputBuffer, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + inputBufferV=inputBuffer + if sys.version_info[0] == 3: + if type(scriptDescription) is str: + scriptDescription=scriptDescription.encode('utf-8') + if type(functionName) is str: + functionName=functionName.encode('utf-8') + if type(inputBuffer) is bytearray: + inputBufferV = (ct.c_ubyte*len(inputBuffer))(*inputBuffer) + if type(inputBuffer) is str: + inputBuffer=inputBuffer.encode('utf-8') + inputBufferV = (ct.c_ubyte*len(inputBuffer))(*inputBuffer) + else: + if type(inputBuffer) is bytearray: + inputBufferV = (ct.c_ubyte*len(inputBuffer))(*inputBuffer) + if type(inputBuffer) is str: + inputBuffer=bytearray(inputBuffer) + inputBufferV = (ct.c_ubyte*len(inputBuffer))(*inputBuffer) + inputBufferV=ct.cast(inputBufferV,ct.POINTER(ct.c_ubyte)) # IronPython needs this + + c_inInts = (ct.c_int*len(inputInts))(*inputInts) + c_inInts = ct.cast(c_inInts,ct.POINTER(ct.c_int)) # IronPython needs this + c_inFloats = (ct.c_float*len(inputFloats))(*inputFloats) + c_inFloats = ct.cast(c_inFloats,ct.POINTER(ct.c_float)) # IronPython needs this + + concatStr=''.encode('utf-8') + for i in range(len(inputStrings)): + a=inputStrings[i] + a=a+'\0' + if type(a) is str: + a=a.encode('utf-8') + concatStr=concatStr+a + c_inStrings = (ct.c_char*len(concatStr))(*concatStr) + + intDataOut =[] + floatDataOut =[] + stringDataOut =[] + bufferOut =bytearray() + + intDataC = ct.c_int() + intDataP = ct.POINTER(ct.c_int)() + floatDataC = ct.c_int() + floatDataP = ct.POINTER(ct.c_float)() + stringDataC = ct.c_int() + stringDataP = ct.POINTER(ct.c_char)() + bufferS = ct.c_int() + bufferP = ct.POINTER(ct.c_ubyte)() + + ret = c_CallScriptFunction(clientID,scriptDescription,options,functionName,len(inputInts),c_inInts,len(inputFloats),c_inFloats,len(inputStrings),c_inStrings,len(inputBuffer),inputBufferV,ct.byref(intDataC),ct.byref(intDataP),ct.byref(floatDataC),ct.byref(floatDataP),ct.byref(stringDataC),ct.byref(stringDataP),ct.byref(bufferS),ct.byref(bufferP),operationMode) + + if ret == 0: + for i in range(intDataC.value): + intDataOut.append(intDataP[i]) + for i in range(floatDataC.value): + floatDataOut.append(floatDataP[i]) + s = 0 + for i in range(stringDataC.value): + a = bytearray() + while stringDataP[s] != b'\0': + if sys.version_info[0] == 3: + a.append(int.from_bytes(stringDataP[s],'big')) + else: + a.append(stringDataP[s]) + s += 1 + s += 1 #skip null + if sys.version_info[0] == 3: + a=str(a,'utf-8') + else: + a=str(a) + stringDataOut.append(a) + for i in range(bufferS.value): + bufferOut.append(bufferP[i]) + if sys.version_info[0] != 3: + bufferOut=str(bufferOut) + + return ret, intDataOut, floatDataOut, stringDataOut, bufferOut + +def simxGetObjectVelocity(clientID, objectHandle, operationMode): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + linearVel = (ct.c_float*3)() + angularVel = (ct.c_float*3)() + ret = c_GetObjectVelocity(clientID, objectHandle, linearVel, angularVel, operationMode) + arr1 = [] + for i in range(3): + arr1.append(linearVel[i]) + arr2 = [] + for i in range(3): + arr2.append(angularVel[i]) + return ret, arr1, arr2 + +def simxPackInts(intList): + ''' + Please have a look at the function description/documentation in the CoppeliaSim user manual + ''' + + if sys.version_info[0] == 3: + s=bytes() + for i in range(len(intList)): + s=s+struct.pack('=1x) +# reserved =sim_simulation_advancing|0x02 +sim_simulation_advancing_lastbeforepause =sim_simulation_advancing|0x03 # Last simulation pass before pause (1x) +sim_simulation_advancing_firstafterpause =sim_simulation_advancing|0x04 # First simulation pass after pause (1x) +sim_simulation_advancing_abouttostop =sim_simulation_advancing|0x05 # "Trying to stop" simulation pass (>=1x) +sim_simulation_advancing_lastbeforestop =sim_simulation_advancing|0x06 # Last simulation pass (1x) + + +# Script execution result (first return value) +sim_script_no_error =0 +sim_script_main_script_nonexistent =1 +sim_script_main_script_not_called =2 +sim_script_reentrance_error =4 +sim_script_lua_error =8 +sim_script_call_error =16 + + + # Script types (serialized!) +sim_scripttype_mainscript =0 +sim_scripttype_childscript =1 +sim_scripttype_jointctrlcallback =4 +sim_scripttype_contactcallback =5 +sim_scripttype_customizationscript =6 +sim_scripttype_generalcallback =7 + +# API call error messages +sim_api_errormessage_ignore =0 # does not memorize nor output errors +sim_api_errormessage_report =1 # memorizes errors (default for C-API calls) +sim_api_errormessage_output =2 # memorizes and outputs errors (default for Lua-API calls) + + +# special argument of some functions +sim_handle_all =-2 +sim_handle_all_except_explicit =-3 +sim_handle_self =-4 +sim_handle_main_script =-5 +sim_handle_tree =-6 +sim_handle_chain =-7 +sim_handle_single =-8 +sim_handle_default =-9 +sim_handle_all_except_self =-10 +sim_handle_parent =-11 + + +# special handle flags +sim_handleflag_assembly =0x400000 +sim_handleflag_model =0x800000 + + +# distance calculation methods (serialized) +sim_distcalcmethod_dl =0 +sim_distcalcmethod_dac =1 +sim_distcalcmethod_max_dl_dac =2 +sim_distcalcmethod_dl_and_dac =3 +sim_distcalcmethod_sqrt_dl2_and_dac2=4 +sim_distcalcmethod_dl_if_nonzero =5 +sim_distcalcmethod_dac_if_nonzero =6 + + + # Generic dialog styles +sim_dlgstyle_message =0 +sim_dlgstyle_input =1 +sim_dlgstyle_ok =2 +sim_dlgstyle_ok_cancel =3 +sim_dlgstyle_yes_no =4 +sim_dlgstyle_dont_center =32# can be combined with one of above values. Only with this flag can the position of the related UI be set just after dialog creation + + # Generic dialog return values +sim_dlgret_still_open =0 +sim_dlgret_ok =1 +sim_dlgret_cancel =2 +sim_dlgret_yes =3 +sim_dlgret_no =4 + + +# Path properties +sim_pathproperty_show_line =0x0001 +sim_pathproperty_show_orientation =0x0002 +sim_pathproperty_closed_path =0x0004 +sim_pathproperty_automatic_orientation =0x0008 +sim_pathproperty_invert_velocity =0x0010 +sim_pathproperty_infinite_acceleration =0x0020 +sim_pathproperty_flat_path =0x0040 +sim_pathproperty_show_position =0x0080 +sim_pathproperty_auto_velocity_profile_translation =0x0100 +sim_pathproperty_auto_velocity_profile_rotation =0x0200 +sim_pathproperty_endpoints_at_zero =0x0400 +sim_pathproperty_keep_x_up =0x0800 + + + # drawing objects +# following are mutually exclusive +sim_drawing_points =0 # 3 values per point (point size in pixels) +sim_drawing_lines =1 # 6 values per line (line size in pixels) +sim_drawing_triangles =2 # 9 values per triangle +sim_drawing_trianglepoints =3 # 6 values per point (3 for triangle position 3 for triangle normal vector) (triangle size in meters) +sim_drawing_quadpoints =4 # 6 values per point (3 for quad position 3 for quad normal vector) (quad size in meters) +sim_drawing_discpoints =5 # 6 values per point (3 for disc position 3 for disc normal vector) (disc size in meters) +sim_drawing_cubepoints =6 # 6 values per point (3 for cube position 3 for cube normal vector) (cube size in meters) +sim_drawing_spherepoints =7 # 3 values per point (sphere size in meters) + +# following can be or-combined +sim_drawing_itemcolors =0x00020 # +3 values per item (each item has its own ambient color (rgb values)). + # Mutually exclusive with sim_drawing_vertexcolors +sim_drawing_vertexcolors =0x00040 # +3 values per vertex (each vertex has its own ambient color (rgb values). Only for sim_drawing_lines (+6) and for sim_drawing_triangles(+9)). Mutually exclusive with sim_drawing_itemcolors +sim_drawing_itemsizes =0x00080 # +1 value per item (each item has its own size). Not for sim_drawing_triangles +sim_drawing_backfaceculling =0x00100 # back faces are not displayed for all items +sim_drawing_wireframe =0x00200 # all items displayed in wireframe +sim_drawing_painttag =0x00400 # all items are tagged as paint (for additinal processing at a later stage) +sim_drawing_followparentvisibility =0x00800 # if the object is associated with a scene object then it follows that visibility otherwise it is always visible +sim_drawing_cyclic =0x01000 # if the max item count was reached then the first items are overwritten. +sim_drawing_50percenttransparency =0x02000 # the drawing object will be 50% transparent +sim_drawing_25percenttransparency =0x04000 # the drawing object will be 25% transparent +sim_drawing_12percenttransparency =0x08000 # the drawing object will be 12.5% transparent +sim_drawing_emissioncolor =0x10000 # When used in combination with sim_drawing_itemcolors or sim_drawing_vertexcolors then the specified colors will be for the emissive component +sim_drawing_facingcamera =0x20000 # Only for trianglepoints quadpoints discpoints and cubepoints. If specified the normal verctor is calculated to face the camera (each item data requires 3 values less) +sim_drawing_overlay =0x40000 # When specified objects are always drawn on top of "regular objects" +sim_drawing_itemtransparency =0x80000 # +1 value per item (each item has its own transparency value (0-1)). Not compatible with sim_drawing_vertexcolors + +# banner values +# following can be or-combined +sim_banner_left =0x00001 # Banners display on the left of the specified point +sim_banner_right =0x00002 # Banners display on the right of the specified point +sim_banner_nobackground =0x00004 # Banners have no background rectangle +sim_banner_overlay =0x00008 # When specified banners are always drawn on top of "regular objects" +sim_banner_followparentvisibility =0x00010 # if the object is associated with a scene object then it follows that visibility otherwise it is always visible +sim_banner_clickselectsparent =0x00020 # if the object is associated with a scene object then clicking the banner will select the scene object +sim_banner_clicktriggersevent =0x00040 # if the banner is clicked an event is triggered (sim_message_eventcallback_bannerclicked and sim_message_bannerclicked are generated) +sim_banner_facingcamera =0x00080 # If specified the banner will always face the camera by rotating around the banner's vertical axis (y-axis) +sim_banner_fullyfacingcamera =0x00100 # If specified the banner will always fully face the camera (the banner's orientation is same as the camera looking at it) +sim_banner_backfaceculling =0x00200 # If specified the banner will only be visible from one side +sim_banner_keepsamesize =0x00400 # If specified the banner will always appear in the same size. In that case size represents the character height in pixels +sim_banner_bitmapfont =0x00800 # If specified a fixed-size bitmap font is used. The text will also always fully face the camera and be right + # to the specified position. Bitmap fonts are not clickable + + +# particle objects following are mutually exclusive +sim_particle_points1 =0 # 6 values per point (pt1 and pt2. Pt1 is start position pt2-pt1 is the initial velocity vector). i + #Point is 1 pixel big. Only appearance is a point internally handled as a perfect sphere +sim_particle_points2 =1 # 6 values per point. Point is 2 pixel big. Only appearance is a point internally handled as a perfect sphere +sim_particle_points4 =2 # 6 values per point. Point is 4 pixel big. Only appearance is a point internally handled as a perfect sphere +sim_particle_roughspheres =3 # 6 values per sphere. Only appearance is rough. Internally a perfect sphere +sim_particle_spheres =4 # 6 values per sphere. Internally a perfect sphere + + + + +# following can be or-combined +sim_particle_respondable1to4 =0x0020 # the particles are respondable against shapes (against all objects that have at least one bit 1-4 activated in the global respondable mask) +sim_particle_respondable5to8 =0x0040 # the particles are respondable against shapes (against all objects that have at least one bit 5-8 activated in the global respondable mask) +sim_particle_particlerespondable =0x0080 # the particles are respondable against each other +sim_particle_ignoresgravity =0x0100 # the particles ignore the effect of gravity. Not compatible with sim_particle_water +sim_particle_invisible =0x0200 # the particles are invisible +sim_particle_itemsizes =0x0400 # +1 value per particle (each particle can have a different size) +sim_particle_itemdensities =0x0800 # +1 value per particle (each particle can have a different density) +sim_particle_itemcolors =0x1000 # +3 values per particle (each particle can have a different color) +sim_particle_cyclic =0x2000 # if the max item count was reached then the first items are overwritten. +sim_particle_emissioncolor =0x4000 # When used in combination with sim_particle_itemcolors then the specified colors will be for the emissive component +sim_particle_water =0x8000 # the particles are water particles (no weight in the water (i.e. when z<0)). Not compatible with sim_particle_ignoresgravity +sim_particle_painttag =0x10000 # The particles can be seen by vision sensors (sim_particle_invisible must not be set) + + + + +# custom user interface menu attributes +sim_ui_menu_title =1 +sim_ui_menu_minimize =2 +sim_ui_menu_close =4 +sim_ui_menu_systemblock =8 + + + +# Boolean parameters +sim_boolparam_hierarchy_visible =0 +sim_boolparam_console_visible =1 +sim_boolparam_collision_handling_enabled =2 +sim_boolparam_distance_handling_enabled =3 +sim_boolparam_ik_handling_enabled =4 +sim_boolparam_gcs_handling_enabled =5 +sim_boolparam_dynamics_handling_enabled =6 +sim_boolparam_joint_motion_handling_enabled =7 +sim_boolparam_path_motion_handling_enabled =8 +sim_boolparam_proximity_sensor_handling_enabled =9 +sim_boolparam_vision_sensor_handling_enabled =10 +sim_boolparam_mill_handling_enabled =11 +sim_boolparam_browser_visible =12 +sim_boolparam_scene_and_model_load_messages =13 +sim_reserved0 =14 +sim_boolparam_shape_textures_are_visible =15 +sim_boolparam_display_enabled =16 +sim_boolparam_infotext_visible =17 +sim_boolparam_statustext_open =18 +sim_boolparam_fog_enabled =19 +sim_boolparam_rml2_available =20 +sim_boolparam_rml4_available =21 +sim_boolparam_mirrors_enabled =22 +sim_boolparam_aux_clip_planes_enabled =23 +sim_boolparam_full_model_copy_from_api =24 +sim_boolparam_realtime_simulation =25 +sim_boolparam_force_show_wireless_emission =27 +sim_boolparam_force_show_wireless_reception =28 +sim_boolparam_video_recording_triggered =29 +sim_boolparam_threaded_rendering_enabled =32 +sim_boolparam_fullscreen =33 +sim_boolparam_headless =34 +sim_boolparam_hierarchy_toolbarbutton_enabled =35 +sim_boolparam_browser_toolbarbutton_enabled =36 +sim_boolparam_objectshift_toolbarbutton_enabled =37 +sim_boolparam_objectrotate_toolbarbutton_enabled=38 +sim_boolparam_force_calcstruct_all_visible =39 +sim_boolparam_force_calcstruct_all =40 +sim_boolparam_exit_request =41 +sim_boolparam_play_toolbarbutton_enabled =42 +sim_boolparam_pause_toolbarbutton_enabled =43 +sim_boolparam_stop_toolbarbutton_enabled =44 +sim_boolparam_waiting_for_trigger =45 + + +# Integer parameters +sim_intparam_error_report_mode =0 # Check sim_api_errormessage_... constants above for valid values +sim_intparam_program_version =1 # e.g Version 2.1.4 --> 20104. Can only be read +sim_intparam_instance_count =2 # do not use anymore (always returns 1 since CoppeliaSim 2.5.11) +sim_intparam_custom_cmd_start_id =3 # can only be read +sim_intparam_compilation_version =4 # 0=evaluation version 1=full version 2=player version. Can only be read +sim_intparam_current_page =5 +sim_intparam_flymode_camera_handle =6 # can only be read +sim_intparam_dynamic_step_divider =7 # can only be read +sim_intparam_dynamic_engine =8 # 0=Bullet 1=ODE. 2=Vortex. +sim_intparam_server_port_start =9 # can only be read +sim_intparam_server_port_range =10 # can only be read +sim_intparam_visible_layers =11 +sim_intparam_infotext_style =12 +sim_intparam_settings =13 +sim_intparam_edit_mode_type =14 # can only be read +sim_intparam_server_port_next =15 # is initialized at sim_intparam_server_port_start +sim_intparam_qt_version =16 # version of the used Qt framework +sim_intparam_event_flags_read =17 # can only be read +sim_intparam_event_flags_read_clear =18 # can only be read +sim_intparam_platform =19 # can only be read +sim_intparam_scene_unique_id =20 # can only be read +sim_intparam_work_thread_count =21 +sim_intparam_mouse_x =22 +sim_intparam_mouse_y =23 +sim_intparam_core_count =24 +sim_intparam_work_thread_calc_time_ms =25 +sim_intparam_idle_fps =26 +sim_intparam_prox_sensor_select_down =27 +sim_intparam_prox_sensor_select_up =28 +sim_intparam_stop_request_counter =29 +sim_intparam_program_revision =30 +sim_intparam_mouse_buttons =31 +sim_intparam_dynamic_warning_disabled_mask =32 +sim_intparam_simulation_warning_disabled_mask =33 +sim_intparam_scene_index =34 +sim_intparam_motionplanning_seed =35 +sim_intparam_speedmodifier =36 + +# Float parameters +sim_floatparam_rand=0 # random value (0.0-1.0) +sim_floatparam_simulation_time_step =1 +sim_floatparam_stereo_distance =2 + +# String parameters +sim_stringparam_application_path=0 # path of CoppeliaSim's executable +sim_stringparam_video_filename=1 +sim_stringparam_app_arg1 =2 +sim_stringparam_app_arg2 =3 +sim_stringparam_app_arg3 =4 +sim_stringparam_app_arg4 =5 +sim_stringparam_app_arg5 =6 +sim_stringparam_app_arg6 =7 +sim_stringparam_app_arg7 =8 +sim_stringparam_app_arg8 =9 +sim_stringparam_app_arg9 =10 +sim_stringparam_scene_path_and_name =13 + +# Array parameters +sim_arrayparam_gravity =0 +sim_arrayparam_fog =1 +sim_arrayparam_fog_color =2 +sim_arrayparam_background_color1=3 +sim_arrayparam_background_color2=4 +sim_arrayparam_ambient_light =5 +sim_arrayparam_random_euler =6 + +sim_objintparam_visibility_layer= 10 +sim_objfloatparam_abs_x_velocity= 11 +sim_objfloatparam_abs_y_velocity= 12 +sim_objfloatparam_abs_z_velocity= 13 +sim_objfloatparam_abs_rot_velocity= 14 +sim_objfloatparam_objbbox_min_x= 15 +sim_objfloatparam_objbbox_min_y= 16 +sim_objfloatparam_objbbox_min_z= 17 +sim_objfloatparam_objbbox_max_x= 18 +sim_objfloatparam_objbbox_max_y= 19 +sim_objfloatparam_objbbox_max_z= 20 +sim_objfloatparam_modelbbox_min_x= 21 +sim_objfloatparam_modelbbox_min_y= 22 +sim_objfloatparam_modelbbox_min_z= 23 +sim_objfloatparam_modelbbox_max_x= 24 +sim_objfloatparam_modelbbox_max_y= 25 +sim_objfloatparam_modelbbox_max_z= 26 +sim_objintparam_collection_self_collision_indicator= 27 +sim_objfloatparam_transparency_offset= 28 +sim_objintparam_child_role= 29 +sim_objintparam_parent_role= 30 +sim_objintparam_manipulation_permissions= 31 +sim_objintparam_illumination_handle= 32 + +sim_visionfloatparam_near_clipping= 1000 +sim_visionfloatparam_far_clipping= 1001 +sim_visionintparam_resolution_x= 1002 +sim_visionintparam_resolution_y= 1003 +sim_visionfloatparam_perspective_angle= 1004 +sim_visionfloatparam_ortho_size= 1005 +sim_visionintparam_disabled_light_components= 1006 +sim_visionintparam_rendering_attributes= 1007 +sim_visionintparam_entity_to_render= 1008 +sim_visionintparam_windowed_size_x= 1009 +sim_visionintparam_windowed_size_y= 1010 +sim_visionintparam_windowed_pos_x= 1011 +sim_visionintparam_windowed_pos_y= 1012 +sim_visionintparam_pov_focal_blur= 1013 +sim_visionfloatparam_pov_blur_distance= 1014 +sim_visionfloatparam_pov_aperture= 1015 +sim_visionintparam_pov_blur_sampled= 1016 +sim_visionintparam_render_mode= 1017 + +sim_jointintparam_motor_enabled= 2000 +sim_jointintparam_ctrl_enabled= 2001 +sim_jointfloatparam_pid_p= 2002 +sim_jointfloatparam_pid_i= 2003 +sim_jointfloatparam_pid_d= 2004 +sim_jointfloatparam_intrinsic_x= 2005 +sim_jointfloatparam_intrinsic_y= 2006 +sim_jointfloatparam_intrinsic_z= 2007 +sim_jointfloatparam_intrinsic_qx= 2008 +sim_jointfloatparam_intrinsic_qy= 2009 +sim_jointfloatparam_intrinsic_qz= 2010 +sim_jointfloatparam_intrinsic_qw= 2011 +sim_jointfloatparam_velocity= 2012 +sim_jointfloatparam_spherical_qx= 2013 +sim_jointfloatparam_spherical_qy= 2014 +sim_jointfloatparam_spherical_qz= 2015 +sim_jointfloatparam_spherical_qw= 2016 +sim_jointfloatparam_upper_limit= 2017 +sim_jointfloatparam_kc_k= 2018 +sim_jointfloatparam_kc_c= 2019 +sim_jointfloatparam_ik_weight= 2021 +sim_jointfloatparam_error_x= 2022 +sim_jointfloatparam_error_y= 2023 +sim_jointfloatparam_error_z= 2024 +sim_jointfloatparam_error_a= 2025 +sim_jointfloatparam_error_b= 2026 +sim_jointfloatparam_error_g= 2027 +sim_jointfloatparam_error_pos= 2028 +sim_jointfloatparam_error_angle= 2029 +sim_jointintparam_velocity_lock= 2030 +sim_jointintparam_vortex_dep_handle= 2031 +sim_jointfloatparam_vortex_dep_multiplication= 2032 +sim_jointfloatparam_vortex_dep_offset= 2033 + +sim_shapefloatparam_init_velocity_x= 3000 +sim_shapefloatparam_init_velocity_y= 3001 +sim_shapefloatparam_init_velocity_z= 3002 +sim_shapeintparam_static= 3003 +sim_shapeintparam_respondable= 3004 +sim_shapefloatparam_mass= 3005 +sim_shapefloatparam_texture_x= 3006 +sim_shapefloatparam_texture_y= 3007 +sim_shapefloatparam_texture_z= 3008 +sim_shapefloatparam_texture_a= 3009 +sim_shapefloatparam_texture_b= 3010 +sim_shapefloatparam_texture_g= 3011 +sim_shapefloatparam_texture_scaling_x= 3012 +sim_shapefloatparam_texture_scaling_y= 3013 +sim_shapeintparam_culling= 3014 +sim_shapeintparam_wireframe= 3015 +sim_shapeintparam_compound= 3016 +sim_shapeintparam_convex= 3017 +sim_shapeintparam_convex_check= 3018 +sim_shapeintparam_respondable_mask= 3019 +sim_shapefloatparam_init_velocity_a= 3020 +sim_shapefloatparam_init_velocity_b= 3021 +sim_shapefloatparam_init_velocity_g= 3022 +sim_shapestringparam_color_name= 3023 +sim_shapeintparam_edge_visibility= 3024 +sim_shapefloatparam_shading_angle= 3025 +sim_shapefloatparam_edge_angle= 3026 +sim_shapeintparam_edge_borders_hidden= 3027 + +sim_proxintparam_ray_invisibility= 4000 + +sim_forcefloatparam_error_x= 5000 +sim_forcefloatparam_error_y= 5001 +sim_forcefloatparam_error_z= 5002 +sim_forcefloatparam_error_a= 5003 +sim_forcefloatparam_error_b= 5004 +sim_forcefloatparam_error_g= 5005 +sim_forcefloatparam_error_pos= 5006 +sim_forcefloatparam_error_angle= 5007 + +sim_lightintparam_pov_casts_shadows= 8000 + +sim_cameraintparam_disabled_light_components= 9000 +sim_camerafloatparam_perspective_angle= 9001 +sim_camerafloatparam_ortho_size= 9002 +sim_cameraintparam_rendering_attributes= 9003 +sim_cameraintparam_pov_focal_blur= 9004 +sim_camerafloatparam_pov_blur_distance= 9005 +sim_camerafloatparam_pov_aperture= 9006 +sim_cameraintparam_pov_blur_samples= 9007 + +sim_dummyintparam_link_type= 10000 + +sim_mirrorfloatparam_width= 12000 +sim_mirrorfloatparam_height= 12001 +sim_mirrorfloatparam_reflectance= 12002 +sim_mirrorintparam_enable= 12003 + +sim_pplanfloatparam_x_min= 20000 +sim_pplanfloatparam_x_range= 20001 +sim_pplanfloatparam_y_min= 20002 +sim_pplanfloatparam_y_range= 20003 +sim_pplanfloatparam_z_min= 20004 +sim_pplanfloatparam_z_range= 20005 +sim_pplanfloatparam_delta_min= 20006 +sim_pplanfloatparam_delta_range= 20007 + +sim_mplanintparam_nodes_computed= 25000 +sim_mplanintparam_prepare_nodes= 25001 +sim_mplanintparam_clear_nodes= 25002 + +# User interface elements +sim_gui_menubar =0x0001 +sim_gui_popups =0x0002 +sim_gui_toolbar1 =0x0004 +sim_gui_toolbar2 =0x0008 +sim_gui_hierarchy =0x0010 +sim_gui_infobar =0x0020 +sim_gui_statusbar =0x0040 +sim_gui_scripteditor =0x0080 +sim_gui_scriptsimulationparameters =0x0100 +sim_gui_dialogs =0x0200 +sim_gui_browser =0x0400 +sim_gui_all =0xffff + + +# Joint modes +sim_jointmode_passive =0 +sim_jointmode_motion =1 +sim_jointmode_ik =2 +sim_jointmode_ikdependent =3 +sim_jointmode_dependent =4 +sim_jointmode_force =5 + + +# Navigation and selection modes with the mouse. Lower byte values are mutually exclusive upper byte bits can be combined +sim_navigation_passive =0x0000 +sim_navigation_camerashift =0x0001 +sim_navigation_camerarotate =0x0002 +sim_navigation_camerazoom =0x0003 +sim_navigation_cameratilt =0x0004 +sim_navigation_cameraangle =0x0005 +sim_navigation_camerafly =0x0006 +sim_navigation_objectshift =0x0007 +sim_navigation_objectrotate =0x0008 +sim_navigation_reserved2 =0x0009 +sim_navigation_reserved3 =0x000A +sim_navigation_jointpathtest =0x000B +sim_navigation_ikmanip =0x000C +sim_navigation_objectmultipleselection =0x000D +# Bit-combine following values and add them to one of above's values for a valid navigation mode +sim_navigation_reserved4 =0x0100 +sim_navigation_clickselection =0x0200 +sim_navigation_ctrlselection =0x0400 +sim_navigation_shiftselection =0x0800 +sim_navigation_camerazoomwheel =0x1000 +sim_navigation_camerarotaterightbutton =0x2000 + + + +#Remote API constants +SIMX_VERSION =0 +# Remote API message header structure +SIMX_HEADER_SIZE =18 +simx_headeroffset_crc =0 # 1 simxUShort. Generated by the client or server. The CRC for the message +simx_headeroffset_version =2 # 1 byte. Generated by the client or server. The version of the remote API software +simx_headeroffset_message_id =3 # 1 simxInt. Generated by the client (and used in a reply by the server) +simx_headeroffset_client_time =7 # 1 simxInt. Client time stamp generated by the client (and sent back by the server) +simx_headeroffset_server_time =11 # 1 simxInt. Generated by the server when a reply is generated. The server timestamp +simx_headeroffset_scene_id =15 # 1 simxUShort. Generated by the server. A unique ID identifying the scene currently displayed +simx_headeroffset_server_state =17 # 1 byte. Generated by the server. Bit coded 0 set --> simulation not stopped 1 set --> simulation paused 2 set --> real-time switch on 3-5 edit mode type (0=no edit mode 1=triangle 2=vertex 3=edge 4=path 5=UI) + +# Remote API command header +SIMX_SUBHEADER_SIZE =26 +simx_cmdheaderoffset_mem_size =0 # 1 simxInt. Generated by the client or server. The buffer size of the command. +simx_cmdheaderoffset_full_mem_size =4 # 1 simxInt. Generated by the client or server. The full buffer size of the command (applies to split chunks). +simx_cmdheaderoffset_pdata_offset0 =8 # 1 simxUShort. Generated by the client or server. The amount of data that is part of the command identification. +simx_cmdheaderoffset_pdata_offset1 =10 # 1 simxInt. Generated by the client or server. The amount of shift of the pure data buffer (applies to split chunks). +simx_cmdheaderoffset_cmd=14 # 1 simxInt. Generated by the client (and used in a reply by the server). The command combined with the operation mode of the command. +simx_cmdheaderoffset_delay_or_split =18 # 1 simxUShort. Generated by the client or server. The amount of delay in ms of a continuous command or the max. pure data size to send at once (applies to split commands). +simx_cmdheaderoffset_sim_time =20 # 1 simxInt. Generated by the server. The simulation time (in ms) when the command was executed (or 0 if simulation is not running) +simx_cmdheaderoffset_status =24 # 1 byte. Generated by the server. (1 bit 0 is set --> error in function execution on server side). The client writes bit 1 if command cannot be overwritten +simx_cmdheaderoffset_reserved =25 # 1 byte. Not yet used + + + + + +# Regular operation modes +simx_opmode_oneshot =0x000000 # sends command as one chunk. Reply will also come as one chunk. Doesn't wait for the reply. +simx_opmode_blocking =0x010000 # sends command as one chunk. Reply will also come as one chunk. Waits for the reply (_REPLY_WAIT_TIMEOUT_IN_MS is the timeout). +simx_opmode_oneshot_wait =0x010000 # sends command as one chunk. Reply will also come as one chunk. Waits for the reply (_REPLY_WAIT_TIMEOUT_IN_MS is the timeout). +simx_opmode_continuous =0x020000 +simx_opmode_streaming =0x020000 # sends command as one chunk. Command will be stored on the server and always executed + #(every x ms (as far as possible) where x can be 0-65535. just add x to opmode_continuous). + # A reply will be sent continuously each time as one chunk. Doesn't wait for the reply. + +# Operation modes for heavy data +simx_opmode_oneshot_split =0x030000 # sends command as several chunks (max chunk size is x bytes where x can be _MIN_SPLIT_AMOUNT_IN_BYTES-65535. Just add x to opmode_oneshot_split). Reply will also come as several chunks. Doesn't wait for the reply. +simx_opmode_continuous_split =0x040000 +simx_opmode_streaming_split =0x040000 # sends command as several chunks (max chunk size is x bytes where x can be _MIN_SPLIT_AMOUNT_IN_BYTES-65535. Just add x to opmode_continuous_split). Command will be stored on the server and always executed. A reply will be sent continuously each time as several chunks. Doesn't wait for the reply. + +# Special operation modes +simx_opmode_discontinue =0x050000 # removes and cancels all commands stored on the client or server side (also continuous commands) +simx_opmode_buffer =0x060000 # doesn't send anything but checks if a reply for the given command is available in the input buffer (i.e. previously received from the server) +simx_opmode_remove =0x070000 # doesn't send anything and doesn't return any specific value. It just erases a similar command reply in the inbox (to free some memory) + + +# Command return codes +simx_return_ok =0x000000 +simx_return_novalue_flag =0x000001 # input buffer doesn't contain the specified command +simx_return_timeout_flag =0x000002 # command reply not received in time for opmode_oneshot_wait operation mode +simx_return_illegal_opmode_flag =0x000004 # command doesn't support the specified operation mode +simx_return_remote_error_flag =0x000008 # command caused an error on the server side +simx_return_split_progress_flag =0x000010 # previous similar command not yet fully processed (applies to opmode_oneshot_split operation modes) +simx_return_local_error_flag =0x000020 # command caused an error on the client side +simx_return_initialize_error_flag =0x000040 # simxStart was not yet called + +# Following for backward compatibility (same as above) +simx_error_noerror =0x000000 +simx_error_novalue_flag =0x000001 # input buffer doesn't contain the specified command +simx_error_timeout_flag =0x000002 # command reply not received in time for opmode_oneshot_wait operation mode +simx_error_illegal_opmode_flag =0x000004 # command doesn't support the specified operation mode +simx_error_remote_error_flag =0x000008 # command caused an error on the server side +simx_error_split_progress_flag =0x000010 # previous similar command not yet fully processed (applies to opmode_oneshot_split operation modes) +simx_error_local_error_flag =0x000020 # command caused an error on the client side +simx_error_initialize_error_flag =0x000040 # simxStart was not yet called + + diff --git a/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/simpleSynchronousTest.py b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/simpleSynchronousTest.py new file mode 100644 index 0000000..a1669ee --- /dev/null +++ b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/simpleSynchronousTest.py @@ -0,0 +1,58 @@ +# This small example illustrates how to use the remote API +# synchronous mode. The synchronous mode needs to be +# pre-enabled on the server side. You would do this by +# starting the server (e.g. in a child script) with: +# +# simRemoteApi.start(19999,1300,false,true) +# +# But in this example we try to connect on port +# 19997 where there should be a continuous remote API +# server service already running and pre-enabled for +# synchronous mode. +# +# +# IMPORTANT: for each successful call to simxStart, there +# should be a corresponding call to simxFinish at the end! + +try: + import sim +except: + print ('--------------------------------------------------------------') + print ('"sim.py" could not be imported. This means very probably that') + print ('either "sim.py" or the remoteApi library could not be found.') + print ('Make sure both are in the same folder as this file,') + print ('or appropriately adjust the file "sim.py"') + print ('--------------------------------------------------------------') + print ('') + +import time +import sys + +print ('Program started') +sim.simxFinish(-1) # just in case, close all opened connections +clientID=sim.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to CoppeliaSim +if clientID!=-1: + print ('Connected to remote API server') + + # enable the synchronous mode on the client: + sim.simxSynchronous(clientID,True) + + # start the simulation: + sim.simxStartSimulation(clientID,sim.simx_opmode_blocking) + + # Now step a few times: + for i in range(1,10): + if sys.version_info[0] == 3: + input('Press key to step the simulation!') + else: + raw_input('Press key to step the simulation!') + sim.simxSynchronousTrigger(clientID); + + # stop the simulation: + sim.simxStopSimulation(clientID,sim.simx_opmode_blocking) + + # Now close the connection to CoppeliaSim: + sim.simxFinish(clientID) +else: + print ('Failed connecting to remote API server') +print ('Program ended') diff --git a/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/simpleTest.py b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/simpleTest.py new file mode 100644 index 0000000..b58bddc --- /dev/null +++ b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/simpleTest.py @@ -0,0 +1,59 @@ +# Make sure to have the server side running in CoppeliaSim: +# in a child script of a CoppeliaSim scene, add following command +# to be executed just once, at simulation start: +# +# simRemoteApi.start(19999) +# +# then start simulation, and run this program. +# +# IMPORTANT: for each successful call to simxStart, there +# should be a corresponding call to simxFinish at the end! + +try: + import sim +except: + print ('--------------------------------------------------------------') + print ('"sim.py" could not be imported. This means very probably that') + print ('either "sim.py" or the remoteApi library could not be found.') + print ('Make sure both are in the same folder as this file,') + print ('or appropriately adjust the file "sim.py"') + print ('--------------------------------------------------------------') + print ('') + +import time + +print ('Program started') +sim.simxFinish(-1) # just in case, close all opened connections +clientID=sim.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to CoppeliaSim +if clientID!=-1: + print ('Connected to remote API server') + + # Now try to retrieve data in a blocking fashion (i.e. a service call): + res,objs=sim.simxGetObjects(clientID,sim.sim_handle_all,sim.simx_opmode_blocking) + if res==sim.simx_return_ok: + print ('Number of objects in the scene: ',len(objs)) + else: + print ('Remote API function call returned with error code: ',res) + + time.sleep(2) + + # Now retrieve streaming data (i.e. in a non-blocking fashion): + startTime=time.time() + sim.simxGetIntegerParameter(clientID,sim.sim_intparam_mouse_x,sim.simx_opmode_streaming) # Initialize streaming + while time.time()-startTime < 5: + returnCode,data=sim.simxGetIntegerParameter(clientID,sim.sim_intparam_mouse_x,sim.simx_opmode_buffer) # Try to retrieve the streamed data + if returnCode==sim.simx_return_ok: # After initialization of streaming, it will take a few ms before the first value arrives, so check the return code + print ('Mouse position x: ',data) # Mouse position x is actualized when the cursor is over CoppeliaSim's window + time.sleep(0.005) + + # Now send some data to CoppeliaSim in a non-blocking fashion: + sim.simxAddStatusbarMessage(clientID,'Hello CoppeliaSim!',sim.simx_opmode_oneshot) + + # Before closing the connection to CoppeliaSim, make sure that the last command sent out had time to arrive. You can guarantee this with (for example): + sim.simxGetPingTime(clientID) + + # Now close the connection to CoppeliaSim: + sim.simxFinish(clientID) +else: + print ('Failed connecting to remote API server') +print ('Program ended') diff --git a/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/synchronousImageTransmission.py b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/synchronousImageTransmission.py new file mode 100644 index 0000000..dd21748 --- /dev/null +++ b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/synchronousImageTransmission.py @@ -0,0 +1,89 @@ +# Make sure to have CoppeliaSim running, with followig scene loaded: +# +# scenes/synchronousImageTransmissionViaRemoteApi.ttt +# +# Do not launch simulation, but run this script +# +# The client side (i.e. this script) depends on: +# +# sim.py, simConst.py, and the remote API library available +# in programming/remoteApiBindings/lib/lib + +try: + import sim +except: + print ('--------------------------------------------------------------') + print ('"sim.py" could not be imported. This means very probably that') + print ('either "sim.py" or the remoteApi library could not be found.') + print ('Make sure both are in the same folder as this file,') + print ('or appropriately adjust the file "sim.py"') + print ('--------------------------------------------------------------') + print ('') + +import math +import time + +class Client: + def __enter__(self): + self.intSignalName='legacyRemoteApiStepCounter' + self.stepCounter=0 + self.lastImageAcquisitionTime=-1 + sim.simxFinish(-1) # just in case, close all opened connections + self.id=sim.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to CoppeliaSim + return self + + def __exit__(self,*err): + sim.simxFinish(-1) + +with Client() as client: + client.runInSynchronousMode=True + + print("running") + + if client.id!=-1: + print ('Connected to remote API server') + + def stepSimulation(): + if client.runInSynchronousMode: + currentStep=client.stepCounter + sim.simxSynchronousTrigger(client.id); + while client.stepCounter==currentStep: + retCode,s=sim.simxGetIntegerSignal(client.id,client.intSignalName,sim.simx_opmode_buffer) + if retCode==sim.simx_return_ok: + client.stepCounter=s + retCode,res,img=sim.simxGetVisionSensorImage(client.id,client.visionSensorHandle,0,sim.simx_opmode_buffer) + client.lastImageAcquisitionTime=sim.simxGetLastCmdTime(client.id) + if retCode==sim.simx_return_ok: + sim.simxSetVisionSensorImage(client.id,client.passiveVisionSensorHandle,img,0,sim.simx_opmode_oneshot) + else: + retCode,res,img=sim.simxGetVisionSensorImage(client.id,client.visionSensorHandle,0,sim.simx_opmode_buffer) + if retCode==sim.simx_return_ok: + imageSimTime=sim.simxGetLastCmdTime(client.id) + if client.lastImageAcquisitionTime!=imageSimTime: + client.lastImageAcquisitionTime=imageSimTime + sim.simxSetVisionSensorImage(client.id,client.passiveVisionSensorHandle,img,0,sim.simx_opmode_oneshot) + + # Start streaming client.intSignalName integer signal, that signals when a step is finished: + sim.simxGetIntegerSignal(client.id,client.intSignalName,sim.simx_opmode_streaming) + + res,client.visionSensorHandle=sim.simxGetObjectHandle(client.id,'VisionSensor',sim.simx_opmode_blocking) + res,client.passiveVisionSensorHandle=sim.simxGetObjectHandle(client.id,'PassiveVisionSensor',sim.simx_opmode_blocking) + + # Start streaming the vision sensor image: + sim.simxGetVisionSensorImage(client.id,client.visionSensorHandle,0,sim.simx_opmode_streaming) + + # enable the synchronous mode on the client: + if client.runInSynchronousMode: + sim.simxSynchronous(client.id,True) + + sim.simxStartSimulation(client.id,sim.simx_opmode_oneshot) + + startTime=time.time() + while time.time()-startTime < 5: + stepSimulation() + + # stop data streaming + sim.simxGetIntegerSignal(client.id,client.intSignalName,sim.simx_opmode_discontinue) + sim.simxGetVisionSensorImage(client.id,client.visionSensorHandle,0,sim.simx_opmode_discontinue) + + sim.simxStopSimulation(client.id,sim.simx_opmode_blocking) diff --git a/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/visualization.py b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/visualization.py new file mode 100644 index 0000000..ff3dbc8 --- /dev/null +++ b/7_Demo_youBotPickAndPlace/code/vrep/VREP_RemoteAPIs/visualization.py @@ -0,0 +1,517 @@ +# -*- coding: utf-8 -*- +"""Code for visualizing data in sim via python. + +Author: Andrew Hundt + +License: Apache v2 https://www.apache.org/licenses/LICENSE-2.0 +""" +import os +import errno +import traceback + +import numpy as np +import six # compatibility between python 2 + 3 = six +import matplotlib.pyplot as plt + +try: + import sim as sim +except Exception as e: + print ('--------------------------------------------------------------') + print ('"sim.py" could not be imported. This means very probably that') + print ('either "sim.py" or the remoteApi library could not be found.') + print ('Make sure both are in PYTHONPATH folder relative to this file,') + print ('or appropriately adjust the file "sim.py. Also follow the"') + print ('ReadMe.txt in the sim remote API folder') + print ('--------------------------------------------------------------') + print ('') + raise e + +import tensorflow as tf + +from tensorflow.python.platform import flags +from tensorflow.python.platform import gfile +from tensorflow.python.ops import data_flow_ops +from ply import write_xyz_rgb_as_ply +from PIL import Image + +# progress bars https://github.com/tqdm/tqdm +# import tqdm without enforcing it as a dependency +try: + from tqdm import tqdm +except ImportError: + + def tqdm(*args, **kwargs): + if args: + return args[0] + return kwargs.get('iterable', None) + +from depth_image_encoding import ClipFloatValues +from depth_image_encoding import FloatArrayToRgbImage +from depth_image_encoding import FloatArrayToRawRGB +from skimage.transform import resize +from skimage import img_as_ubyte +from skimage import img_as_uint +from skimage.color import grey2rgb + +try: + import eigen # https://github.com/jrl-umi3218/Eigen3ToPython + import sva # https://github.com/jrl-umi3218/SpaceVecAlg +except ImportError: + print('eigen and sva python modules are not available. To install run the script at:' + 'https://github.com/ahundt/robotics_setup/blob/master/robotics_tasks.sh' + 'or follow the instructions at https://github.com/jrl-umi3218/Eigen3ToPython' + 'and https://github.com/jrl-umi3218/SpaceVecAlg. ' + 'When you build the modules make sure python bindings are enabled.') + +tf.flags.DEFINE_string('csimVisualizeDepthFormat', 'csim_depth_encoded_rgb', + """ Controls how Depth images are displayed. Options are: + None: Do not modify the data and display it as-is for rgb input data (not working properly for float depth). + 'depth_rgb': convert a floating point depth image to a straight 0-255 encoding of depths less than 3m + 'depth_encoded_rgb': convert a floating point depth image to the rgb encoding used by + the google brain robot data grasp dataset's raw png depth image encoding, + see https://sites.google.com/site/brainrobotdata/home/depth-image-encoding. + 'sim': add a sim prefix to any of the above commands to + rotate image by 180 degrees, flip left over right, then invert the color channels + after the initial conversion step. + This is due to a problem where CoppeliaSim seems to display images differently. + Examples include 'csim_depth_rgb' and 'csim_depth_encoded_rgb', + see http://forum.coppeliarobotics.com/viewtopic.php?f=9&t=737&p=27805#p27805. + """) +tf.flags.DEFINE_string('csimVisualizeRGBFormat', 'csim_rgb', + """ Controls how images are displayed. Options are: + None: Do not modify the data and display it as-is for rgb input data (not working properly for float depth). + 'depth_rgb': convert a floating point depth image to a straight 0-255 encoding of depths less than 3m + 'depth_encoded_rgb': convert a floating point depth image to the rgb encoding used by + the google brain robot data grasp dataset's raw png depth image encoding, + see https://sites.google.com/site/brainrobotdata/home/depth-image-encoding. + 'sim': add a sim prefix to any of the above commands to + rotate image by 180 degrees, flip left over right, then invert the color channels + after the initial conversion step. + This is due to a problem where CoppeliaSim seems to display images differently. + Examples include 'csim_depth_rgb' and 'csim_depth_encoded_rgb', + see http://forum.coppeliarobotics.com/viewtopic.php?f=9&t=737&p=27805#p27805. + """) + +# the following line is needed for tf versions before 1.5 +# flags.FLAGS._parse_flags() +FLAGS = flags.FLAGS + + +def depth_image_to_point_cloud(depth, intrinsics_matrix, dtype=np.float32, verbose=0): + """Depth images become an XYZ point cloud in the camera frame with shape (depth.shape[0], depth.shape[1], 3). + + Transform a depth image into a point cloud in the camera frame with one point for each + pixel in the image, using the camera transform for a camera + centred at cx, cy with field of view fx, fy. + + Based on: + https://github.com/tensorflow/models/blob/master/research/cognitive_mapping_and_planning/src/depth_utils.py + https://codereview.stackexchange.com/a/84990/10101 + + also see grasp_geometry_tf.depth_image_to_point_cloud(). + + # Arguments + + depth: is a 2-D ndarray with shape (rows, cols) containing + 32bit floating point depths in meters. The result is a 3-D array with + shape (rows, cols, 3). Pixels with invalid depth in the input have + NaN or 0 for the z-coordinate in the result. + + intrinsics_matrix: 3x3 matrix for projecting depth values to z values + in the point cloud frame. http://ksimek.github.io/2013/08/13/intrinsic/ + In this case x0, y0 are at index [2, 0] and [2, 1], respectively. + + transform: 4x4 Rt matrix for rotating and translating the point cloud + """ + fy = intrinsics_matrix[1, 1] + fx = intrinsics_matrix[0, 0] + # center of image y coordinate + center_y = intrinsics_matrix[2, 1] + # center of image x coordinate + center_x = intrinsics_matrix[2, 0] + depth = np.squeeze(depth) + y_range, x_range = depth.shape + + y, x = np.meshgrid(np.arange(y_range), + np.arange(x_range), + indexing='ij') + assert y.size == x.size and y.size == depth.size + x = x.flatten() + y = y.flatten() + depth = depth.flatten() + + X = (x - center_x) * depth / fx + Y = (y - center_y) * depth / fy + + assert X.size == Y.size and X.size == depth.size + assert X.shape == Y.shape and X.shape == depth.shape + + if verbose > 0: + print('X np: ', X.shape) + print('Y np: ', Y.shape) + print('depth np: ', depth.shape) + XYZ = np.column_stack([X, Y, depth]) + assert XYZ.shape == (y_range * x_range, 3) + if verbose > 0: + print('XYZ pre reshape np: ', XYZ.shape) + XYZ = XYZ.reshape((y_range, x_range, 3)) + + return XYZ.astype(dtype) + + +def csimPrint(client_id, message): + """Print a message in both the python command line and on the CoppeliaSim Statusbar. + + The Statusbar is the white command line output on the bottom of the CoppeliaSim GUI window. + """ + sim.simxAddStatusbarMessage(client_id, message, sim.simx_opmode_oneshot) + print(message) + + +def create_dummy(client_id, display_name, transform=None, parent_handle=-1, debug=FLAGS.csimDebugMode, operation_mode=sim.simx_opmode_blocking): + """Create a dummy object in the simulation + + # Arguments + + transform_display_name: name string to use for the object in the sim scene + transform: 3 cartesian (x, y, z) and 4 quaternion (x, y, z, w) elements, same as sim + parent_handle: -1 is the world frame, any other int should be a sim object handle + """ + if transform is None: + transform = np.array([0., 0., 0., 0., 0., 0., 1.]) + # 2. Now create a dummy object at coordinate 0.1,0.2,0.3 with name 'MyDummyName': + empty_buffer = bytearray() + res, ret_ints, ret_floats, ret_strings, ret_buffer = sim.simxCallScriptFunction( + client_id, + 'remoteApiCommandServer', + sim.sim_scripttype_childscript, + 'createDummy_function', + [parent_handle], + transform, + [display_name], + empty_buffer, + operation_mode) + if res == sim.simx_return_ok: + # display the reply from CoppeliaSim (in this case, the handle of the created dummy) + if debug is not None and 'print_transform' in debug: + print ('Dummy name:', display_name, ' handle: ', ret_ints[0], ' transform: ', transform) + else: + print('create_dummy remote function call failed.') + print(''.join(traceback.format_stack())) + return -1 + return ret_ints[0] + + +def setPose(client_id, display_name, transform=None, parent_handle=-1): + """Set the pose of an object in the simulation + + # Arguments + + transform_display_name: name string to use for the object in the sim scene + transform: 3 cartesian (x, y, z) and 4 quaternion (x, y, z, w) elements, same as sim + parent_handle: -1 is the world frame, any other int should be a sim object handle + """ + if transform is None: + transform = np.array([0., 0., 0., 0., 0., 0., 1.]) + # 2. Now create a dummy object at coordinate 0.1,0.2,0.3 with name 'MyDummyName': + empty_buffer = bytearray() + res, ret_ints, ret_floats, ret_strings, ret_buffer = sim.simxCallScriptFunction( + client_id, + 'remoteApiCommandServer', + sim.sim_scripttype_childscript, + 'createDummy_function', + [parent_handle], + transform, + [display_name], + empty_buffer, + sim.simx_opmode_blocking) + if res == sim.simx_return_ok: + # display the reply from CoppeliaSim (in this case, the handle of the created dummy) + print ('SetPose object name:', display_name, ' handle: ', ret_ints[0], ' transform: ', transform) + else: + print('setPose remote function call failed.') + print(''.join(traceback.format_stack())) + return -1 + return ret_ints[0] + + +def set_vision_sensor_image(client_id, display_name, image, convert=None, scale_factor=256000.0, operation_mode=sim.simx_opmode_oneshot_wait): + """Display vision sensor image data in a CoppeliaSim simulation. + + [CoppeliaSim Vision Sensors](http://www.coppeliarobotics.com/helpFiles/en/visionSensors.htm) + [simSetVisionSensorImage](http://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm#simSetVisionSensorImage) + + # Arguments + + display_name: the string display name of the sensor object in the CoppeliaSim scene + image: an rgb char array containing an image + convert: Controls how images are displayed. Options are: + None: Do not modify the data and display it as-is for rgb input data (not working properly for float depth). + 'depth_rgb': convert a floating point depth image to a straight 0-255 encoding of depths less than 3m + 'depth_encoded_rgb': convert a floating point depth image to the rgb encoding used by + the google brain robot data grasp dataset's raw png depth image encoding, + see https://sites.google.com/site/brainrobotdata/home/depth-image-encoding. + 'sim': add a sim prefix to any of the above commands to + rotate image by 180 degrees, flip left over right, then invert the color channels + after the initial conversion step. + This is due to a problem where CoppeliaSim seems to display images differently. + Examples include 'csim_depth_rgb' and 'csim_depth_encoded_rgb', + see http://www.forum.coppeliarobotics.com/viewtopic.php?f=9&t=737&p=27805#p27805. + """ + strings = [display_name] + parent_handle = -1 + + # TODO(ahundt) support is_greyscale True again + is_greyscale = 0 + csim_conversion = False + if convert is not None: + csim_conversion = 'sim' in convert + + if 'depth_encoded_rgb' in convert: + image = np.array(FloatArrayToRgbImage(image, scale_factor=scale_factor, drop_blue=False), dtype=np.uint8) + elif 'depth_rgb' in convert: + + image = img_as_uint(image) + + elif not csim_conversion: + raise ValueError('set_vision_sensor_image() convert parameter must be one of `depth_encoded_rgb`, `depth_rgb`, or None' + 'with the optional addition of the word `sim` to rotate 180, flip left right, then invert colors.') + + if csim_conversion: + # rotate 180 degrees, flip left over right, then invert the colors + image = np.array(256 - np.fliplr(np.rot90(image, 2)), dtype=np.uint8) + + if np.issubdtype(image.dtype, np.integer): + is_float = 0 + floats = [] + color_buffer = bytearray(image.flatten().tobytes()) + color_size = image.size + num_floats = 0 + else: + is_float = 1 + floats = [image] + color_buffer = bytearray() + num_floats = image.size + color_size = 0 + + cloud_handle = -1 + res, ret_ints, ret_floats, ret_strings, ret_buffer = sim.simxCallScriptFunction( + client_id, + 'remoteApiCommandServer', + sim.sim_scripttype_childscript, + 'setVisionSensorImage_function', + [parent_handle, num_floats, is_greyscale, color_size], # int params + np.append(floats, []), # float params + strings, # string params + # byte buffer params + color_buffer, + operation_mode) + if res == sim.simx_return_ok: + print ('point cloud handle: ', ret_ints[0]) # display the reply from CoppeliaSim (in this case, the handle of the created dummy) + # set the transform for the point cloud + return ret_ints[0] + else: + print('insertPointCloud_function remote function call failed.') + print(''.join(traceback.format_stack())) + return res + + +def create_point_cloud(client_id, display_name, transform=None, point_cloud=None, depth_image=None, color_image=None, + camera_intrinsics_matrix=None, parent_handle=-1, clear=True, + max_voxel_size=0.01, max_point_count_per_voxel=10, point_size=10, options=8, + rgb_sensor_display_name=None, depth_sensor_display_name=None, convert_depth=FLAGS.csimVisualizeDepthFormat, + convert_rgb=FLAGS.csimVisualizeRGBFormat, save_ply_path=None, rgb_display_mode='vision_sensor'): + """Create a point cloud object in the simulation, plus optionally render the depth and rgb images. + + # Arguments + + display_name: name string to use for the object in the sim scene + depth_image: A depth image of size [width, height, 3] + transform: [x, y, z, qw, qx, qy, qz] with 3 cartesian (x, y, z) and 4 quaternion (qx, qy, qz, qw) elements, same as sim + This transform is from the parent handle to the point cloud base + parent_handle: -1 is the world frame, any other int should be a sim object handle + clear: clear the point cloud if it already exists with the provided display name + maxVoxelSize: the maximum size of the octree voxels containing points + maxPtCntPerVoxel: the maximum number of points allowed in a same octree voxel + options: bit-coded: + bit0 set (1): points have random colors + bit1 set (2): show octree structure + bit2 set (4): reserved. keep unset + bit3 set (8): do not use an octree structure. When enabled, point cloud operations are limited, and point clouds will not be collidable, measurable or detectable anymore, but adding points will be much faster + bit4 set (16): color is emissive + pointSize: the size of the points, in pixels + reserved: reserved for future extensions. Set to NULL + save_ply_path: save out a ply file with the point cloud data + point_cloud: optional XYZ point cloud of size [width, height, 3], will be generated if not provided. + convert_rgb: Controls how images are displayed. Options are: + None: Do not modify the data and display it as-is for rgb input data (not working properly for float depth). + 'depth_rgb': convert a floating point depth image to a straight 0-255 encoding of depths less than 3m + 'depth_encoded_rgb': convert a floating point depth image to the rgb encoding used by + the google brain robot data grasp dataset's raw png depth image encoding, + see https://sites.google.com/site/brainrobotdata/home/depth-image-encoding. + 'sim': add a sim prefix to any of the above commands to + rotate image by 180 degrees, flip left over right, then invert the color channels + after the initial conversion step. + This is due to a problem where CoppeliaSim seems to display images differently. + Examples include 'csim_depth_rgb' and 'csim_depth_encoded_rgb', + see http://www.forum.coppeliarobotics.com/viewtopic.php?f=9&t=737&p=27805#p27805. + rgb_display_mode: Options help with working around quirks in input image data's layout. + 'point_cloud' to display the image when the point cloud is being generated. + 'vision_sensor' to make a separate call go the vison sensor display function. + """ + if transform is None: + transform = np.array([0., 0., 0., 0., 0., 0., 1.]) + + if point_cloud is None: + point_cloud = depth_image_to_point_cloud(depth_image, camera_intrinsics_matrix) + point_cloud = point_cloud.reshape([point_cloud.size/3, 3]) + + # show the depth sensor image + if depth_sensor_display_name is not None and depth_image is not None: + # matplotlib.image.imsave(display_name + depth_sensor_display_name + '_norotfliplr.png', depth_image) + # rotate 180, flip left over right then invert the image colors for display in CoppeliaSim + # depth_image = np.fliplr(np.rot90(depth_image, 2)) + # matplotlib.image.imsave(display_name + depth_sensor_display_name + '_rot90fliplr.png', depth_image) + set_vision_sensor_image(client_id, depth_sensor_display_name, depth_image, convert=convert_depth) + + # show the rgb sensor image, this overwrites the rgb display + # done in insertPointCloud_function, which is buggy + if rgb_sensor_display_name is not None and color_image is not None and rgb_display_mode == 'vision_sensor': + # matplotlib.image.imsave(display_name + rgb_sensor_display_name + '_norotfliplr.png', color_image) + # rotate 180, flip left over right then invert the image colors for display in CoppeliaSim + # matplotlib.image.imsave(display_name + rgb_sensor_display_name + '_rot90fliplr.png', color_image) + set_vision_sensor_image(client_id, rgb_sensor_display_name, color_image, convert=convert_rgb) + + # Save out Point cloud + if save_ply_path is not None: + write_xyz_rgb_as_ply(point_cloud, color_image, save_ply_path) + + # color_buffer is initially empty + color_buffer = bytearray() + strings = [display_name] + if rgb_sensor_display_name is not None and rgb_display_mode == 'point_cloud': + strings = [display_name, rgb_sensor_display_name] + + transform_entries = 7 + if clear: + clear = 1 + else: + clear = 0 + + cloud_handle = -1 + # Create the point cloud if it does not exist, or retrieve the handle if it does + res, ret_ints, ret_floats, ret_strings, ret_buffer = sim.simxCallScriptFunction( + client_id, + 'remoteApiCommandServer', + sim.sim_scripttype_childscript, + 'createPointCloud_function', + # int params + [parent_handle, transform_entries, point_cloud.size, cloud_handle, clear, max_point_count_per_voxel, options, point_size], + # float params + [max_voxel_size], + # string params + strings, + # byte buffer params + color_buffer, + sim.simx_opmode_blocking) + + setPose(client_id, display_name, transform, parent_handle) + + if res == sim.simx_return_ok: + cloud_handle = ret_ints[0] + + # convert the rgb values to a string + color_size = 0 + if color_image is not None: + # see simInsertPointsIntoPointCloud() in sim documentation + # 3 indicates the cloud should be in the parent frame, and color is enabled + # bit 2 is 1 so each point is colored + simInsertPointsIntoPointCloudOptions = 3 + # color_buffer = bytearray(np.fliplr(np.rot90(color_image, 3)).flatten().tobytes()) + color_buffer = bytearray(color_image.flatten().tobytes()) + color_size = color_image.size + else: + simInsertPointsIntoPointCloudOptions = 1 + + # Actually transfer the point cloud + res, ret_ints, ret_floats, ret_strings, ret_buffer = sim.simxCallScriptFunction( + client_id, + 'remoteApiCommandServer', + sim.sim_scripttype_childscript, + 'insertPointCloud_function', + [parent_handle, transform_entries, point_cloud.size, cloud_handle, color_size, simInsertPointsIntoPointCloudOptions], + np.append(point_cloud, []), + strings, + color_buffer, + sim.simx_opmode_blocking) + + if res == sim.simx_return_ok: + print ('point cloud handle: ', ret_ints[0]) # display the reply from CoppeliaSim (in this case, the handle of the created dummy) + # set the transform for the point cloud + return ret_ints[0] + else: + print('insertPointCloud_function remote function call failed.') + print(''.join(traceback.format_stack())) + return res + + else: + print('createPointCloud_function remote function call failed') + print(''.join(traceback.format_stack())) + return res + + +def drawLines(client_id, display_name, lines, parent_handle=-1, transform=None, debug=FLAGS.csimDebugMode, operation_mode=sim.simx_opmode_blocking): + """Create a line in the simulation. + + Note that there are currently some quirks with this function. Only one line is accepted, + and sometimes CoppeliaSim fails to delete the object correctly and lines will fail to draw. + In that case you need to close and restart CoppeliaSim. + + # Arguments + + transform_display_name: name string to use for the object in the sim scene + transform: 3 cartesian (x, y, z) and 4 quaternion (x, y, z, w) elements, same as sim + parent_handle: -1 is the world frame, any other int should be a sim object handle + lines: array of line definitions using two endpoints (x0, y0, z0, x1, y1, z1). + Multiple lines can be defined but there should be 6 entries (two points) per line. + """ + # 2. Now create a dummy object at coordinate 0.1,0.2,0.3 with name 'MyDummyName': + empty_buffer = bytearray() + res, ret_ints, ret_floats, ret_strings, ret_buffer = sim.simxCallScriptFunction( + client_id, + 'remoteApiCommandServer', + sim.sim_scripttype_childscript, + 'addDrawingObject_function', + [parent_handle, int(lines.size/6)], + # np.append(transform, lines), + lines, + [display_name], + empty_buffer, + operation_mode) + if res == sim.simx_return_ok: + # display the reply from CoppeliaSim (in this case, the handle of the created dummy) + if debug is not None and 'print_drawLines' in debug: + print ('drawLines name:', display_name, ' handle: ', ret_ints[0], ' transform: ', transform) + + if transform is not None: + # set the transform for the point cloud + setPose(client_id, display_name, transform, parent_handle) + else: + print('drawLines remote function call failed.') + print(''.join(traceback.format_stack())) + return -1 + return ret_ints[0] + + +def restore_cropped(cropped_image, crop_size, crop_offset, full_size): + """ Restore cropped_image to full size image with zero padding + First scale image back to crop_size, then padding + """ + + cropped_image = np.squeeze(cropped_image) + restored = np.zeros((full_size[0], full_size[1]), dtype=cropped_image.dtype) + scaled_crop = resize(cropped_image, (crop_size[0], crop_size[1])) + restored[crop_offset[0]:crop_offset[0]+crop_size[0], + crop_offset[1]:crop_offset[1]+crop_size[1]] = scaled_crop + + return restored diff --git a/7_Demo_youBotPickAndPlace/code/vrep/demoRemoteControl.py b/7_Demo_youBotPickAndPlace/code/vrep/demoRemoteControl.py new file mode 100644 index 0000000..cfc5800 --- /dev/null +++ b/7_Demo_youBotPickAndPlace/code/vrep/demoRemoteControl.py @@ -0,0 +1,130 @@ +import time +import math +import sys +sys.path.append('./VREP_remoteAPIs') + +''' Inverse kinematics ''' +def chassisInverseKinematics(vx, vy, omega, wheel_R, a, b): + omega_1 = (vy - vx + (a+b)*omega)/wheel_R + omega_2 = (vy + vx - (a+b)*omega)/wheel_R + omega_3 = (vy - vx - (a+b)*omega)/wheel_R + omega_4 = (vy + vx + (a+b)*omega)/wheel_R + + # set the direction for each wheel + v_wheel = [0,0,0,0] + v_wheel[0] = -omega_1 + v_wheel[1] = -omega_2 + v_wheel[2] = -omega_3 + v_wheel[3] = -omega_4 + + return v_wheel + +''' Arm control function of youBot ''' +def VREP_armControl(vrep_sim, clientID, arm_joints_handle, desired_arm_joint_angles): + for i in range(0,5): + vrep_sim.simxSetJointPosition(clientID, arm_joints_handle[i], desired_arm_joint_angles[i], vrep_sim.simx_opmode_blocking) + +''' Wheels control function of youBot ''' +def VREP_wheelsControl(vrep_sim, clientID, wheel_joints_handle, desired_wheel_velocities): + for i in range(0,4): + vrep_sim.simxSetJointTargetVelocity(clientID, wheel_joints_handle[i], desired_wheel_velocities[i], vrep_sim.simx_opmode_blocking) + + +if __name__ == '__main__': + try: + import sim as vrep_sim + except: + print ('--------------------------------------------------------------') + print ('"sim.py" could not be imported. This means very probably that') + print ('either "sim.py" or the remoteApi library could not be found.') + print ('Make sure both are in the same folder as this file,') + print ('or appropriately adjust the file "sim.py"') + print ('--------------------------------------------------------------') + print ('') + + ''' Initialization ''' + print ('Program started') + vrep_sim.simxFinish(-1) # just in case, close all opened connections + clientID = vrep_sim.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to CoppeliaSim + if clientID != -1: + print ('Connected to remote API server') + + return_code, youBot_handle = vrep_sim.simxGetObjectHandle(clientID, 'youBot', vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object youBot ok.') + + return_code, youBot_dummy_handle = vrep_sim.simxGetObjectHandle(clientID, 'youBotDummy', vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object youBotDummy ok.') + + # Prepare initial values for four wheels + wheel_joints_handle = [-1,-1,-1,-1]; # front left, rear left, rear right, front right + return_code, wheel_joints_handle[0] = vrep_sim.simxGetObjectHandle(clientID, 'rollingJoint_fr', vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object youBot rollingJoint_fl ok.') + + return_code, wheel_joints_handle[1] = vrep_sim.simxGetObjectHandle(clientID, 'rollingJoint_fl', vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object youBot rollingJoint_rl ok.') + + return_code, wheel_joints_handle[2] = vrep_sim.simxGetObjectHandle(clientID, 'rollingJoint_rl', vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object youBot rollingJoint_rr ok.') + + return_code, wheel_joints_handle[3] = vrep_sim.simxGetObjectHandle(clientID, 'rollingJoint_rr', vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object youBot rollingJoint_fr ok.') + + # Prepare initial values for five arm joints + arm_joints_handle = [-1,-1,-1,-1,-1] + for i in range(0,4): + return_code, arm_joints_handle[i] = vrep_sim.simxGetObjectHandle(clientID, 'youBotArmJoint' + str(i), vrep_sim.simx_opmode_blocking) + if (return_code == vrep_sim.simx_return_ok): + print('get object arm joint ' + str(i) + ' ok.') + + # Desired joint positions for initialization + desired_arm_joint_angles = [180*math.pi/180, 30.91*math.pi/180, 52.42*math.pi/180, 72.68*math.pi/180, 0] + + # Initialization all arm joints + for i in range(0,4): + vrep_sim.simxSetJointPosition(clientID, arm_joints_handle[i], desired_arm_joint_angles[i], vrep_sim.simx_opmode_blocking) + else: + print ('Failed connecting to remote API server') + + + # Size information for youBot + wheel_R = 0.05 + a = 0.165 + b = 0.228 + + # User variables + simu_time = 0 + center_velocity = [0,0,0] + desired_wheel_velocities = [0,0,0,0] + + ''' Main control loop ''' + print('begin main control loop ...') + while True: + # Motion planning + simu_time = simu_time + 0.05 + + for i in range(0,5): + if int(simu_time) % 2 == 0: + desired_arm_joint_angles[i] = desired_arm_joint_angles[i] - 0.04 # rad + else: + desired_arm_joint_angles[i] = desired_arm_joint_angles[i] + 0.04 # rad + + # Control the youBot robot + VREP_armControl(vrep_sim, clientID, arm_joints_handle, desired_arm_joint_angles) + VREP_wheelsControl(vrep_sim, clientID, wheel_joints_handle, desired_wheel_velocities) + + + # Now send some data to CoppeliaSim in a non-blocking fashion: + vrep_sim.simxAddStatusbarMessage(clientID,'Over!',vrep_sim.simx_opmode_oneshot) + + # Before closing the connection to CoppeliaSim, make sure that the last command sent out had time to arrive. You can guarantee this with (for example): + vrep_sim.simxGetPingTime(clientID) + + # Now close the connection to CoppeliaSim: + vrep_sim.simxFinish(clientID) + print ('Program ended') diff --git a/7_Demo_youBotPickAndPlace/vrep_scenes/youBot_pick_and_place.ttt b/7_Demo_youBotPickAndPlace/vrep_scenes/youBot_pick_and_place.ttt new file mode 100644 index 0000000..4fd1098 Binary files /dev/null and b/7_Demo_youBotPickAndPlace/vrep_scenes/youBot_pick_and_place.ttt differ diff --git a/7_Demo_youBotPickAndPlace/youBotPickAndPlace.gif b/7_Demo_youBotPickAndPlace/youBotPickAndPlace.gif new file mode 100644 index 0000000..5ded6c4 Binary files /dev/null and b/7_Demo_youBotPickAndPlace/youBotPickAndPlace.gif differ diff --git a/README.md b/README.md index d78a777..6a5b7e2 100644 --- a/README.md +++ b/README.md @@ -50,7 +50,11 @@ YouBot机器人的第一个简单控制代码，可以作为入门级学习代 ![image](https://github.com/chauby/V-REP-YouBot-Demo/blob/master/6_Demo_MatlabAndPythonControl/MatlabPythonVREP.gif) +## 7. Demo_MatlabAndPythonControl +使用Python编写控制代码远程控制V-REP中的YouBot机器人模型完成从A点抓取物体并移动到B点的任务。 + +![image](https://github.com/chauby/V-REP-YouBot-Demo/blob/master/7_Demo_youBotPickAndPlace/youBotPickAndPlace.gif) ## 文章教程