def trivial_transformation(clientID, x, y, z): relative_pos = [x, y, z] vrep.simxCreateDummy(clientID, 0, None, vrep.simx_opmode_blocking) opcode, handle = vrep.simxGetObjectHandle(clientID, "Dummy#", vrep.simx_opmode_blocking) opcode, left_zed = vrep.simxGetObjectHandle(clientID, "zed_vision1#", vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, handle, left_zed, relative_pos, vrep.simx_opmode_blocking) opcode, absolute_pos = vrep.simxGetObjectPosition( clientID, handle, -1, vrep.simx_opmode_blocking) return absolute_pos
def trivial_transformation(clientID, x, y, z): global initialized relative_pos = [x, y, z] if initialized is False: vrep.simxCreateDummy(clientID, 0, None, vrep.simx_opmode_blocking) initialized = True opcode, handle = vrep.simxGetObjectHandle(clientID, "Dummy0", vrep.simx_opmode_blocking) opcode, left_zed = vrep.simxGetObjectHandle(clientID, "zed_vision1", vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, handle, left_zed, relative_pos, vrep.simx_opmode_blocking) opcode, absolute_pos = vrep.simxGetObjectPosition( clientID, handle, -1, vrep.simx_opmode_blocking) return absolute_pos
def add_dummy(clientID, size, color, theta, base_joint_handle): S = np.array([[0., 0., 0., 0., 0., 0., 0.], [0., 1., 0., -1., 0., 1., 0.], [1., 0., 1., 0., 1., 0., 1.], [0., -0.34, 0., 0.74, 0., -1.14, 0.], [0., 0., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 0., 0.]]) errorCode, dummy_handle = vrep.simxCreateDummy(clientID, 0.1, color, vrep.simx_opmode_oneshot_wait) end_position = forward_kinematics.forwardKinematics(theta)[0:3,3:4] vrep.simxSetObjectPosition(clientID, dummy_handle, base_joint_handle, end_position, vrep.simx_opmode_oneshot_wait) return dummy_handle
def notifyCollision(clientID, collision, offset): result, handle = vrep.simxCreateDummy(clientID, 0.3, None, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, handle, -1, [1.5, -2.5 + (0.35 * offset), 0], vrep.simx_opmode_oneshot) return handle
def get_dummy_handle(): dummy = "Dummy" # dummy_handles.append(get_obj_handle(dummy, vrep.simx_opmode_blocking)) for i in range(0, p_robot.shape[1]): print(i) # dummy_handles.append(get_obj_handle(dummy + str(i), vrep.simx_opmode_blocking)) return_code, handle = vrep.simxCreateDummy(clientID, dummy_diameter[0][i], None, vrep.simx_opmode_blocking) check_error(return_code, "dummy", 1) dummy_handles.append(handle)
def notifyCollision(clientID, collision, offset): # Create red dummy if in collsion if collision: result, handle = vrep.simxCreateDummy(clientID, 0.3, [255, 0, 0], vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, handle, -1, [1.5, -2.5 + (0.35 * offset), 0], vrep.simx_opmode_oneshot) return handle # Create green dummy if in collision else: result, handle = vrep.simxCreateDummy(clientID, 0.3, [0, 255, 0], vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, handle, -1, [1.5, -2.5 + (0.35 * offset), 0], vrep.simx_opmode_oneshot) return handle
def create_pose(transform_matrix=numpy.eye(4)): ''' Creates a shape of type 'dummy' in the scene :param transform_matrix: :return: None ''' size = 0.01 res, dummy_handle = vrep.simxCreateDummy(clientID, size, None, vrep.simx_opmode_blocking) __check_resp(res, vrep.simx_return_ok) position = util.get_position_from_matrix(transform_matrix) quat = util.get_quat_from_matrix(transform_matrix) res = vrep.simxSetObjectQuaternion(clientID, dummy_handle, -1, quat, vrep.simx_opmode_blocking) __check_resp(res, vrep.simx_return_ok) res = vrep.simxSetObjectPosition(clientID, dummy_handle, -1, position, vrep.simx_opmode_blocking) __check_resp(res, vrep.simx_return_ok)
def create_dummy(self, pos): retCode, dummyHandle = vrep.simxCreateDummy(self.client_id, 0.2, (128, 128, 128), vrep.simx_opmode_blocking) self.set_object_position(dummyHandle, pos)
# Loop through all the robot's joints and get unique ID's for each joint_handles = [] for i in range(1, 8): joint_name = 'LBR_iiwa_7_R800_joint' + str(i) errorCode, handle = vrep.simxGetObjectHandle(clientID, joint_name, vrep.simx_opmode_oneshot_wait) joint_handles.append(handle) # Get a handle for the movable inverse kinematics dummy errorCode, target_dummy_handle = vrep.simxGetObjectHandle(clientID, "Dummy", vrep.simx_opmode_oneshot_wait) # Adding robot dummies representing bounding volumes onto joint robot_joint_bounding_handles = [] for joint in range(7): # Make a Dummy for each joint if joint == 5: errorCode, bounding_handle = vrep.simxCreateDummy(clientID, 0.10, [125,125,125], vrep.simx_opmode_oneshot_wait) elif joint == 6: errorCode, bounding_handle = vrep.simxCreateDummy(clientID, 0.08, [125,125,125], vrep.simx_opmode_oneshot_wait) else: errorCode, bounding_handle = vrep.simxCreateDummy(clientID, 0.15, [125,125,125], vrep.simx_opmode_oneshot_wait) robot_joint_bounding_handles.append(bounding_handle) vrep.simxSetObjectParent(clientID, robot_joint_bounding_handles[joint], joint_handles[joint], False, vrep.simx_opmode_oneshot_wait) # Kevin # # print("Building wall!") # wall_handles = [] # # for z_pos in range(1, 2): # for z_pos in range(1, 12, 2): # for x_pos in range(-10, 10, 2): # # for x_pos in range(1, 2): # errorCode, bounding_handle = vrep.simxCreateDummy(clientID, 0.10, [200,200,200], vrep.simx_opmode_oneshot_wait)
self.parent = parent # Close all open connections (just in case) vrep.simxFinish(-1) # Connect to V-REP (raise exception on failure) clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if clientID == -1: raise Exception('Failed connecting to remote API server') ################################################################################ inital_S = [] # Dummies for Cuboids result, dummy_0 = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_blocking) # Get handle for the Cuboid result, Cuboid_handle = vrep.simxGetObjectHandle(clientID, 'Cuboid', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for the Cuboid') # Get position of the Cuboid result, Cuboid_position = vrep.simxGetObjectPosition(clientID, Cuboid_handle, -1, vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('Could not get object position for the Cuboid') Cuboid_position = np.reshape(Cuboid_position, (3, 1)) # Position of the cuboid # print ("Cuboid position", Cuboid_position)
def perform_IK(T_1, M, S, dof, frame_selection, Larm_joints, Rarm_joints, body_joints, joint_bodynames, joint_Rarm, joint_Larm, clientID): #syntax follows homework syntax theta = np.random.rand(dof, 1).tolist() theta = [theta[0] for theta in theta] samples = 10 V_threshold = 0.1 theta_threshold = 0.01 theta_err = 10 S_bracketlist = [] rot, pos = create_rottransmatrix(T_1) rot_angles = rotMatrix2Euler(rot) rot_angles = [rot_angles[0] for angle in rot_angles] vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot) # Wait for simulation to settle down time.sleep(2) res, dummy = vrep.simxCreateDummy(clientID, 0.25, [], vrep.simx_opmode_blocking) res, base_handler = vrep.simxGetObjectHandle(clientID, 'Baxter_base_visible', vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, dummy, base_handler, pos, vrep.simx_opmode_blocking) vrep.simxSetObjectOrientation(clientID, dummy, base_handler, rot_angles, vrep.simx_opmode_blocking) time.sleep(2) for vals in S.T: s_bracket = create_bracket(vals) S_bracketlist.append(s_bracket) for i in range(1, samples): if frame_selection == 1: movebody(clientID, body_joints, joint_bodynames, theta) elif frame_selection == 2: move_leftarm(clientID, Larm_joints, joint_Larm, theta) elif frame_selection == 3: move_rightarm(clientID, Rarm_joints, joint_Rarm, theta) S_explist, T_2 = perform_FK(S_bracketlist, theta, M) combined_T = np.dot(T_2, np.linalg.inv(T_1)) V = la.logm(combined_T) V_screw = mat2screw(V) V_screw = np.asarray(V_screw) V_err = la.norm(V_screw) if V_err < V_threshold or theta_err < theta_threshold: rot, pos = create_rottransmatrix(T_2) rot_angles = rotMatrix2Euler(rot) rot_angles = [rot_angles[0] for angle in rot_angles] res, dummy2 = vrep.simxCreateDummy(clientID, 0.25, [0, 255, 0], vrep.simx_opmode_blocking) res, base_handler = vrep.simxGetObjectHandle( clientID, 'Baxter_base_visible', vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, dummy2, base_handler, pos, vrep.simx_opmode_blocking) vrep.simxSetObjectOrientation(clientID, dummy2, base_handler, rot_angles, vrep.simx_opmode_blocking) return theta J = get_Jacobian(S_explist, S) theta_dot = np.dot(la.pinv(J.T), V_screw) theta_err = np.linalg.norm(theta_dot) theta = theta - (theta_dot) print "Simulation did not converge" return theta
for i in range(len(paths[0])): foundPath[i] = vert[paths[0][i]] # Voronoi graph visualisation in V-Rep # black = [0, 0, 0, 0, 0, 0, 64, 64, 64, 0, 0, 0] # maxCost = max(costs) # for i in range(1, len(vert)-1): # if costs[i] == -1: # _, temp = vrep.simxCreateDummy(clientID, 0.1, black, vrep.simx_opmode_oneshot_wait) # else: # color = ut.get_color(costs[i]/maxCost)+[0, 0, 0, 64, 64, 64, 0, 0, 0] # _, temp = vrep.simxCreateDummy(clientID, 0.1, color, vrep.simx_opmode_oneshot_wait) # vrep.simxSetObjectPosition(clientID, temp, -1, vert[i], vrep.simx_opmode_oneshot_wait) # path visualisation in V-Rep blue = [0, 0, 255, 0, 0, 0, 64, 64, 64, 0, 0, 0] for i in range(1, len(foundPath) - 1): _, temp = vrep.simxCreateDummy(clientID, 0.15, blue, vrep.simx_opmode_oneshot_wait) vrep.simxSetObjectPosition(clientID, temp, -1, foundPath[i], vrep.simx_opmode_oneshot_wait) # start boids program t = boids.start(clientID, quads, targets, speed, sensors, foundPath, None, leader) print('Operation time: ' + str(t) + 's\n') # end program and close the connection print('Simulation end') vrep.simxFinish(clientID)
def create_dummy(self, pos: List[float], size: float = 0.2): ret, dummy_handle = vrep.simxCreateDummy(self.id, size, None, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(self.id, dummy_handle, -1, pos, vrep.simx_opmode_blocking)
def main(args): # Close all open connections (just in case) vrep.simxFinish(-1) # Connect to V-REP (raise exception on failure) clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if clientID == -1: raise Exception('Failed connecting to remote API server') # Start simulation vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot) MLeft = np.array([[-1, 0, 0, -0.1278], [0, 0, 1, 1.3363], [0, 1, 0, 1.2445], [0, 0, 0, 1]]) SLeft = np.zeros((6, 8)) SLeft[:, 0] = screw(0, 0, 1, 0.0103, 0.0147, 0) SLeft[:, 1] = screw(0, 0, 1, -0.1278, 0.2630, 0) SLeft[:, 2] = screw(-1, 0, 0, -0.1278, 0.310, 1.3244) SLeft[:, 3] = screw(0, 1, 0, -0.1278, 0.4140, 1.3244) SLeft[:, 4] = screw(-1, 0, 0, -0.1278, 0.6765, 1.2554) SLeft[:, 5] = screw(0, 1, 0, -0.1278, 0.7801, 1.2554) SLeft[:, 6] = screw(-1, 0, 0, -0.1278, 1.0508, 1.2454) SLeft[:, 7] = screw(0, 1, 0, -0.1278, 1.1667, 1.2454) MRight = np.array([[0, 0, 1, 1.332], [1, 0, 0, -0.12287], [0, 1, 0, 1.2445], [0, 0, 0, 1]]) SRight = np.zeros((6, 8)) SRight[:, 0] = screw(0, 0, 1, 0.0103, 0.0147, 0) SRight[:, 1] = screw(0, 0, 1, 0.2387, -0.1230, 0) SRight[:, 2] = screw(0, 1, 0, 0.3077, -0.1230, 1.3244) SRight[:, 3] = screw(1, 0, 0, 0.4097, -0.1230, 1.3244) SRight[:, 4] = screw(0, 1, 0, 0.6722, -0.1230, 1.2554) SRight[:, 5] = screw(1, 0, 0, 0.7758, -0.1230, 1.2554) SRight[:, 6] = screw(0, 1, 0, 1.0465, -0.1230, 1.2454) SRight[:, 7] = screw(1, 0, 0, 1.1624, -0.1230, 1.2454) obstacle_centers = [] body_centers = [] arm_centers = [] for j in range(len(obstacle_names)): result, dummy_handle = vrep.simxGetObjectHandle( clientID, obstacle_names[j], vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception("Could not get object handle for the Dummy object") status, position = vrep.simxGetObjectPosition( clientID, dummy_handle, -1, vrep.simx_opmode_blocking) obstacle_centers.append(np.array(position)) for k in range(len(body_names)): result, dummy_handle = vrep.simxGetObjectHandle( clientID, body_names[k], vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception("Could not get object handle for the Dummy object") status, position = vrep.simxGetObjectPosition( clientID, dummy_handle, -1, vrep.simx_opmode_blocking) body_centers.append(np.array(position)) for h in range(len(arm_names)): result, dummy_handle = vrep.simxGetObjectHandle( clientID, arm_names[h], vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception("Could not get object handle for the Dummy object") status, position = vrep.simxGetObjectPosition( clientID, dummy_handle, -1, vrep.simx_opmode_blocking) arm_centers.append(np.array(position)) result, table_handle = vrep.simxGetObjectHandle(clientID, "customizableTable", vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception("Could not get object handle for the Table object") status, table_position = vrep.simxGetObjectPosition( clientID, table_handle, -1, vrep.simx_opmode_blocking) dummies = [] for i in range(13): x_offset = np.array([i * 0.1 - 0.6, 0, 0]) for j in range(9): y_offset = np.array([0, j * 0.1 - 0.4, 0]) result, handle = vrep.simxCreateDummy(clientID, 0.1, [0, 0, 255], vrep.simx_opmode_blocking) dummies.append(handle) vrep.simxSetObjectPosition( clientID, handle, -1, np.array(table_position) + x_offset + y_offset, vrep.simx_opmode_oneshot) obstacle_centers.append( np.array(table_position) + x_offset + y_offset) obstacle_names.append("dummy") obstacle_diam.append(0.1) arm = "left" if arm == "left": M = MLeft S = SLeft elif arm == "right": M = MRight S = SRight else: print("Please enter left or right for the arm") return result, dumbell_handle = vrep.simxGetObjectHandle( clientID, "15lbDumbell0", vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception("Could not get object handle for the Dumbell object") # Go above the weight status, position = vrep.simxGetObjectPosition(clientID, dumbell_handle, -1, vrep.simx_opmode_blocking) pose = np.eye(4) pose[0:3, 3] = np.array(position) + np.array([0, -0.2, 0.25]) rotation = np.array([[-1, 0, 0], [0, 0, 1], [0, 1, 0]]) pose[0:3, 0:3] = rotation moveArmAndFrame(clientID, M, S, SLeft, SRight, arm_centers, body_centers, obstacle_centers, pose, arm, "ReferenceFrame0") # Go just in front of the weight pose2 = np.eye(4) pose2[0:3, 3] = np.array(position) + np.array([0, -0.2, 0]) rotation = np.array([[-1, 0, 0], [0, 0, 1], [0, 1, 0]]) pose2[0:3, 0:3] = rotation moveArmAndFrame(clientID, M, S, SLeft, SRight, arm_centers, body_centers, obstacle_centers, pose2, arm, "ReferenceFrame1", plan=False) # Get closer to the weight pose3 = np.eye(4) pose3[0:3, 3] = np.array(position) + np.array([0, -0.1, 0]) rotation = np.array([[-1, 0, 0], [0, 0, 1], [0, 1, 0]]) pose3[0:3, 0:3] = rotation moveArmAndFrame(clientID, M, S, SLeft, SRight, arm_centers, body_centers, obstacle_centers, pose3, arm, "ReferenceFrame2", plan=False) time.sleep(1) # Close the hands vrep.simxSetIntegerSignal(clientID, "leftGripperClose", 1, vrep.simx_opmode_oneshot) #vrep.simxSetIntegerSignal(clientID, "rightGripperClose", 1, vrep.simx_opmode_oneshot) time.sleep(3) result, joint_handle = vrep.simxGetObjectHandle(clientID, "Baxter_leftArm_joint2", vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception( "Could not get object handle for {} arm joint {}".format( arm, i + 1)) # Set the desired value of the joint variable vrep.simxSetJointTargetPosition(clientID, joint_handle, -3, vrep.simx_opmode_oneshot) time.sleep(3) # Back to zero position thetas = [degToRad(-170), 0, 0, 0, 0, 0, 0, 0] moveTorso(clientID, thetas[0]) moveArmsAndFrames(clientID, MLeft, SLeft, MRight, SRight, thetas[1:]) time.sleep(3) # Curl thetas = [ degToRad(-170), degToRad(-45), degToRad(60), degToRad(170), 0, 0, 0, degToRad(220) ] for i in range(10): moveTorso(clientID, thetas[0]) moveArm(arm, clientID, thetas[1:]) if i == 0: time.sleep(3) thetas[4] += degToRad(15) time.sleep(2) # Back to zero position thetas = [degToRad(-170), 0, 0, 0, 0, 0, 0, 0] moveTorso(clientID, thetas[0]) moveArmsAndFrames(clientID, MLeft, SLeft, MRight, SRight, thetas[1:]) # Dab moveArm("left", clientID, [ -0.35672754, -1.05517622, -1.43581716, 0.96819909, -0.85928368, 1.30047417, -1.54662531 ]) time.sleep(2) moveArm("right", clientID, [ 0.29879645, -0.77735934, -1.1740211, 0.44697439, -1.1613436, -0.00886994, 1.1017402 ]) time.sleep(2) # Open the hands vrep.simxSetIntegerSignal(clientID, "leftGripperClose", 0, vrep.simx_opmode_oneshot) #vrep.simxSetIntegerSignal(clientID, "rightGripperClose", 0, vrep.simx_opmode_oneshot) time.sleep(10) clearNotifications(clientID, dummies) # Stop simulation vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot) # Before closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example): vrep.simxGetPingTime(clientID) # Close the connection to V-REP vrep.simxFinish(clientID)
def deriveFK(clientID, Larm_theta, Rarm_theta, body_theta, Larm_joints, Rarm_joints, body_joints): #convert each theta into radians Larm_thetarad = [] Rarm_thetarad = [] body_thetarad = [] if Larm_theta != []: for theta in Larm_theta: theta = int(theta) * (np.pi / 180) Larm_thetarad.append(theta) if Rarm_theta != []: for theta in Rarm_theta: theta = int(theta) * (np.pi / 180) Rarm_thetarad.append(theta) if body_theta != []: for theta in body_theta: theta = int(theta) * (np.pi / 180) body_thetarad.append(theta) dummy_handler = [] dummyres = [] #declare handler parent res, base_handler = vrep.simxGetObjectHandle(clientID, 'Baxter_base_visible', vrep.simx_opmode_blocking) res, Larm_handler = vrep.simxGetObjectHandle(clientID, 'Baxter_leftArm_tip', vrep.simx_opmode_blocking) res, Rarm_handler = vrep.simxGetObjectHandle(clientID, 'BaxterGripper', vrep.simx_opmode_blocking) res, monitor_handler = vrep.simxGetObjectHandle(clientID, 'Baxter_monitor', vrep.simx_opmode_blocking) #Display Coordinate frame via dummy res, dummy_monitor = vrep.simxCreateDummy(clientID, 0.1, [], vrep.simx_opmode_blocking) dummyres.append(res) dummy_handler.append(dummy_monitor) res2, dummy_Rarm = vrep.simxCreateDummy(clientID, 0.1, [], vrep.simx_opmode_blocking) dummyres.append(res2) dummy_handler.append(dummy_Rarm) res3, dummy_Larm = vrep.simxCreateDummy(clientID, 0.1, [], vrep.simx_opmode_blocking) dummyres.append(res3) dummy_handler.append(dummy_Larm) res4, dummy_base = vrep.simxCreateDummy(clientID, 0.25, [], vrep.simx_opmode_blocking) #Will clean up later (place into loops) positions = [] orientations = [] result, posB = vrep.simxGetObjectPosition(clientID, base_handler, base_handler, vrep.simx_opmode_blocking) result, orientationB = vrep.simxGetObjectOrientation( clientID, base_handler, base_handler, vrep.simx_opmode_blocking) result, posL = vrep.simxGetObjectPosition(clientID, Larm_handler, base_handler, vrep.simx_opmode_blocking) result, orientationL = vrep.simxGetObjectOrientation( clientID, Larm_handler, base_handler, vrep.simx_opmode_blocking) positions.append(posL) orientations.append(orientationL) result, posR = vrep.simxGetObjectPosition(clientID, Rarm_handler, base_handler, vrep.simx_opmode_blocking) result, orientationR = vrep.simxGetObjectOrientation( clientID, Rarm_handler, base_handler, vrep.simx_opmode_blocking) positions.append(posR) orientations.append(orientationR) result, posM = vrep.simxGetObjectPosition(clientID, monitor_handler, base_handler, vrep.simx_opmode_blocking) result, orientationM = vrep.simxGetObjectOrientation( clientID, monitor_handler, base_handler, vrep.simx_opmode_blocking) positions.append(posM) orientations.append(orientationM) #Set Original Tool Frame positions vrep.simxSetObjectPosition(clientID, dummy_base, base_handler, posB, vrep.simx_opmode_blocking) vrep.simxSetObjectOrientation(clientID, dummy_base, base_handler, orientationB, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, dummy_Larm, base_handler, posL, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, dummy_Rarm, base_handler, posR, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, dummy_monitor, base_handler, posM, vrep.simx_opmode_blocking) vrep.simxSetObjectOrientation(clientID, dummy_Larm, base_handler, orientationL, vrep.simx_opmode_blocking) vrep.simxSetObjectOrientation(clientID, dummy_Rarm, base_handler, orientationR, vrep.simx_opmode_blocking) vrep.simxSetObjectOrientation(clientID, dummy_monitor, base_handler, orientationM, vrep.simx_opmode_blocking) #T_matrixlist = obtain_homogeneousTmatrix(clientID, positions, orientations) for result in dummyres: if result != vrep.simx_return_ok: raise Exception('Could not get Dummy') return dummy_handler, Larm_thetarad, Rarm_thetarad, body_thetarad
print('Make sure both are in the same folder as this file,') print('or appropriately adjust the file "vrep.py"') print('--------------------------------------------------------------') print('') import time print('KUKA MOVER started') vrep.simxFinish(-1) # just in case, close all opened connections clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to V-REP if clientID != -1: print('Connected to KUKA Simulator') # Make a Dummy errorCode, dummy_handle_0 = vrep.simxCreateDummy( clientID, 0.1, None, vrep.simx_opmode_oneshot_wait) # Joint: 1 2 3 4 5 6 7 theta_list = np.array([1.0, 1.57, 1.0, 1.57, 0.0, 1.57, 0.0]) for i in range(7): theta_list[i] = float( input("Enter number for joint {}: ".format(i + 1))) # theta_list[i] = (theta_list[i] * math.pi) / 180 # List to store our joint handles in joint_handles = [] # Loop through all the robot's joints and get unique ID's for each for i in range(7): joint_name = 'LBR_iiwa_7_R800_joint' + str(i + 1)
# Get handles for all the movable collision detection dummies errorCode, movable_dummy_handle1 = vrep.simxGetObjectHandle( clientID, "Dummy1", vrep.simx_opmode_oneshot_wait) errorCode, movable_dummy_handle2 = vrep.simxGetObjectHandle( clientID, "Dummy2", vrep.simx_opmode_oneshot_wait) errorCode, movable_dummy_handle3 = vrep.simxGetObjectHandle( clientID, "Dummy3", vrep.simx_opmode_oneshot_wait) errorCode, movable_dummy_handle4 = vrep.simxGetObjectHandle( clientID, "Dummy4", vrep.simx_opmode_oneshot_wait) errorCode, movable_dummy_handle5 = vrep.simxGetObjectHandle( clientID, "Dummy5", vrep.simx_opmode_oneshot_wait) # Create bounding volume dummies for the joints BOUNDING_VOL_RADIUS = 0.15 END_RADIUS = 0.05 errorCode, bounding_vol1_handle = vrep.simxCreateDummy( clientID, BOUNDING_VOL_RADIUS, None, vrep.simx_opmode_oneshot_wait) errorCode, bounding_vol2_handle = vrep.simxCreateDummy( clientID, BOUNDING_VOL_RADIUS, None, vrep.simx_opmode_oneshot_wait) errorCode, bounding_vol3_handle = vrep.simxCreateDummy( clientID, BOUNDING_VOL_RADIUS, None, vrep.simx_opmode_oneshot_wait) errorCode, bounding_vol4_handle = vrep.simxCreateDummy( clientID, BOUNDING_VOL_RADIUS, None, vrep.simx_opmode_oneshot_wait) errorCode, bounding_vol5_handle = vrep.simxCreateDummy( clientID, BOUNDING_VOL_RADIUS, None, vrep.simx_opmode_oneshot_wait) errorCode, bounding_vol6_handle = vrep.simxCreateDummy( clientID, BOUNDING_VOL_RADIUS, None, vrep.simx_opmode_oneshot_wait) errorCode, bounding_vol7_handle = vrep.simxCreateDummy( clientID, END_RADIUS, None, vrep.simx_opmode_oneshot_wait) # Initialize variable which let us check if the dummy has been moved prev_dummy_pos = None
import vrep import math import numpy as np R = 30 clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) _, dsethandle = vrep.simxCreateDummy(clientID, 0.5, None, vrep.simx_opmode_blocking) ''' for i in range(0,100): _, dhandle = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_blocking) _ = vrep.simxSetObjectParent(clientID,dhandle,dsethandle,True,vrep.simx_opmode_blocking) _ = vrep.simxSetObjectPosition(clientID, dhandle, -1, [-50 + i * 0.5, 0, 0], vrep.simx_opmode_oneshot) for i in range(0,100): _, dhandle = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_blocking) _ = vrep.simxSetObjectParent(clientID, dhandle, dsethandle, True, vrep.simx_opmode_blocking) _ = vrep.simxSetObjectPosition(clientID, dhandle, -1, [0, 0 + i*0.5, 0], vrep.simx_opmode_oneshot) for i in range(0,100): print("i = " + str(i)) _, dhandle = vrep.simxGetObjectHandle(clientID,"Dummy"+str(100+i),vrep.simx_opmode_blocking) _ = vrep.simxSetObjectPosition(clientID,dhandle,-1,[0,i*0.5,0],vrep.simx_opmode_streaming) _ = vrep.simxSetObjectOrientation(clientID,dhandle,-1,[0,0,math.pi/2],vrep.simx_opmode_oneshot) ''' ''' for theta in np.arange(0,2*math.pi,0.5/R): _, dhandle = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_blocking) _ = vrep.simxSetObjectParent(clientID, dhandle, dsethandle, True, vrep.simx_opmode_blocking)
jointPoses = parseUrdfJoint(urdfTree, robotJointNames, robotTree) vrep.simxFinish(-1) # 关闭所有连接 clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # 连接到VREP if clientID != -1: print('Connected to remote API server') # 关闭动力学 vrep.simxSetBooleanParameter(clientID, vrep.sim_boolparam_dynamics_handling_enabled, False, vrep.simx_opmode_blocking) virtualJHandles = [] for jname, jpose in jointPoses.items(): translate, angles = jpose res, dummy = vrep.simxCreateDummy(clientID, 0.05, None, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, dummy, -1, translate, vrep.simx_opmode_blocking) vrep.simxSetObjectOrientation(clientID, dummy, -1, angles, vrep.simx_opmode_blocking) virtualJHandles.append(dummy) # 1.显示消息并返回反馈 emptyBuff = bytearray() # clientID,脚本依附对象名称,脚本类型,函数名称,输入int型,输入float型,输入string型,输入bytearray型(应当为空),阻赛模式 # returnCode,输出int型,输出float型,输出string型,输出bytearray res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction( clientID, 'remoteApiCommandServer', vrep.sim_scripttype_childscript, 'writeRobot', virtualJHandles, [], robotLinkNames, emptyBuff, vrep.simx_opmode_blocking) if res == vrep.simx_return_ok: