def pickup(clientID, connector, object_sensor): result, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor( clientID, object_sensor, vrep.simx_opmode_buffer) #print(result, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector) attached_shape = None if detectionState == 1: close_gripper(clientID) attached_shape = detectedObjectHandle vrep.simxSetObjectParent(clientID, attached_shape, connector, True, vrep.simx_opmode_blocking) return attached_shape
def render_human(self, chair, position, offset): x, person = vrep.simxLoadModel(self.clientID, self.models_path + "Sitting Bill.ttm", 0, vrep.simx_opmode_blocking) x = vrep.simxSetObjectParent(self.clientID, person, chair, True, vrep.simx_opmode_blocking) offset_z = vrep.simxGetObjectFloatParameter( self.clientID, person, vrep.sim_objfloatparam_modelbbox_max_z, vrep.simx_opmode_blocking)[1] vrep.simxSetObjectPosition(self.clientID, person, -1, (position[0] + offset[0], position[1] + offset[1], position[2] + offset[2]), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(self.clientID, person, -1, (offset[3], offset[4], offset[5]), vrep.simx_opmode_oneshot) # vrep.simxSetModelProperty(self.clientID,person,vrep.sim_modelproperty_not_collidable,vrep.simx_opmode_oneshot) # x, objs = vrep.simxGetObjects(self.clientID, vrep.sim_handle_all, vrep.simx_opmode_oneshot_wait) # x, joint = vrep.simxGetObjectHandle(self.clientID, 'Bill_leftElbowJoint', vrep.simx_opmode_oneshot_wait) # x, shoulder = vrep.simxGetObjectHandle(self.clientID, 'Bill_leftShoulderJoint', vrep.simx_opmode_oneshot_wait) # rotationMatrix = (-1,0,0,0,0,-1,0,0,0,0,-1,0) # x = vrep.simxSetSphericalJointMatrix(self.clientID, shoulder, rotationMatrix,vrep.simx_opmode_streaming) #Moving Rotational Joint # ang = -math.pi/2 # x = vrep.simxSetJointPosition(self.clientID, joint, ang, vrep.simx_opmode_oneshot) # x, pos = vrep.simxGetJointPosition(self.clientID, joint, vrep.simx_opmode_streaming) return person
def render_chair(self, table, position, offset): x, chair = vrep.simxLoadModel(self.clientID, self.models_path + "dining chair.ttm", 0, vrep.simx_opmode_blocking) x = vrep.simxSetObjectParent(self.clientID, chair, table, True, vrep.simx_opmode_blocking) offset_z = math.fabs( vrep.simxGetObjectFloatParameter( self.clientID, chair, vrep.sim_objfloatparam_modelbbox_min_z, vrep.simx_opmode_blocking)[1]) vrep.simxSetObjectPosition( self.clientID, chair, -1, (position[0] + offset[0], position[1] + offset[1], position[2] + offset_z + offset[2]), vrep.simx_opmode_oneshot) vrep.simxSetObjectOrientation(self.clientID, chair, -1, (offset[3], offset[4], offset[5]), vrep.simx_opmode_oneshot) #returnCode, prop = vrep.simxGetModelProperty(self.clientID,chair,vrep.simx_opmode_blocking) # print (prop, prop + vrep.sim_modelproperty_not_respondable + vrep.sim_modelproperty_not_collidable) #returnCode2 = vrep.simxSetModelProperty(self.clientID, chair, prop + vrep.sim_modelproperty_not_respondable, vrep.simx_opmode_blocking) #returnCode3, prop = vrep.simxGetModelProperty(self.clientID,chair,vrep.simx_opmode_blocking) # print (returnCode, returnCode2, returnCode3, prop) # vrep.simxSetModelProperty(self.clientID,chair,vrep.sim_modelproperty_not_collidable,vrep.simx_opmode_oneshot) return chair
def set_parent(self, parent, keep_pos=True): """Set object parent.""" if self._handle < 0: if self._handle == MISSING_HANDLE: raise RuntimeError("Could not set parent of {}: missing name " "or handle.".format(self._name)) if self._handle == REMOVED_OBJ_HANDLE: raise RuntimeError("Could not set parent of {}: object " "removed.".format(self._name)) client_id = self.client_id if client_id is None: raise ConnectionError( "Could not set parent of {}: not connected to V-REP remote " "API server.".format(self._name)) parent_handle = to_handle(parent, "parent") res = vrep.simxSetObjectParent(client_id, self._handle, parent_handle, keep_pos, vrep.simx_opmode_blocking) if res != vrep.simx_return_ok: raise ServerError("Could not set parent of {}.".format(self._name)) if self._parent is not None: self._parent.unregister_child(self) self._parent = None if isinstance(parent, SceneObject): parent.register_child(self) self._parent = parent
def fechaGarra(clientID, num): _, j1 = vrep.simxGetObjectHandle(clientID, 'junta_m1', vrep.simx_opmode_blocking) _, j2 = vrep.simxGetObjectHandle(clientID, 'junta_m2', vrep.simx_opmode_blocking) vrep.simxPauseCommunication(clientID, True) vrep.simxSetJointTargetPosition(clientID, j1, 0.028, vrep.simx_opmode_oneshot) vrep.simxSetJointTargetPosition(clientID, j2, 0.028, vrep.simx_opmode_oneshot) vrep.simxPauseCommunication(clientID, False) _, s = vrep.simxGetObjectHandle(clientID, 'fixaGarra', vrep.simx_opmode_blocking) _, cubo = vrep.simxGetObjectHandle(clientID, 'Cubo' + str(num), vrep.simx_opmode_blocking) time.sleep(1) vrep.simxSetObjectParent(clientID, cubo, s, True, vrep.simx_opmode_blocking)
def abreGarra(clientID, num): _, mesa = vrep.simxGetObjectHandle(clientID, 'mesa', vrep.simx_opmode_blocking) _, cubo = vrep.simxGetObjectHandle(clientID, 'Cubo' + str(num), vrep.simx_opmode_blocking) vrep.simxSetObjectParent(clientID, cubo, mesa, True, vrep.simx_opmode_blocking) _, j1 = vrep.simxGetObjectHandle(clientID, 'junta_m1', vrep.simx_opmode_blocking) _, j2 = vrep.simxGetObjectHandle(clientID, 'junta_m2', vrep.simx_opmode_blocking) vrep.simxPauseCommunication(clientID, True) vrep.simxSetJointTargetPosition(clientID, j1, 0, vrep.simx_opmode_oneshot) vrep.simxSetJointTargetPosition(clientID, j2, 0, vrep.simx_opmode_oneshot) vrep.simxPauseCommunication(clientID, False) time.sleep(1) return 0
# 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) # wall_handles.append(bounding_handle) # vrep.simxSetObjectPosition(clientID,bounding_handle,-1,[0.05 * x_pos, 0.3 , 0.05 * z_pos],vrep.simx_opmode_oneshot_wait) # # for i in range(len(wall_handles)): # errorCode, sphere_pos = vrep.simxGetObjectPosition(clientID, wall_handles[i], -1, vrep.simx_opmode_oneshot_wait) # p_obstacle_list.append(sphere_pos)
import vrep import math import numpy as np R=15 clientID = vrep.simxStart('127.0.0.1',19997,True,True,5000,5) _, dsethandle = vrep.simxCreateDummy(clientID, 0.5, None, vrep.simx_opmode_blocking) for x in np.arange(0,R*2,0.5): _, dhandle = vrep.simxCreateDummy(clientID, 0.3, [255,0,0], vrep.simx_opmode_blocking) _ = vrep.simxSetObjectParent(clientID,dhandle,dsethandle,True,vrep.simx_opmode_blocking) _ = vrep.simxSetObjectPosition(clientID, dhandle, -1, [x+25 ,math.sqrt(R**2-(x-R)**2), 0], vrep.simx_opmode_oneshot) for x in np.arange(2*R,0,-0.5): _, dhandle = vrep.simxCreateDummy(clientID, 0.3, [255,0,0], vrep.simx_opmode_blocking) _ = vrep.simxSetObjectParent(clientID,dhandle,dsethandle,True,vrep.simx_opmode_blocking) _ = vrep.simxSetObjectPosition(clientID, dhandle, -1, [x+25 ,-math.sqrt(R**2-(x-R)**2), 0], vrep.simx_opmode_oneshot) _, colHandle = vrep.simxGetCollectionHandle(clientID,"Ref",vrep.simx_opmode_blocking) _, refHandle, intData, doubleData, strData = vrep.simxGetObjectGroupData(clientID,colHandle,3,vrep.simx_opmode_blocking) refPos = np.reshape(doubleData,(len(refHandle),3)) for index in range(0,len(refHandle)-1): curPos = refPos[index] nextPos = refPos[index+1] relPos = nextPos-curPos
def grip_off(): # err, pos = vrep.simxGetObjectPosition(clientID, cube, table, vrep.simx_opmode_blocking) vrep.simxSetObjectParent(clientID, cube, -1, 1, vrep.simx_opmode_blocking) # vrep.simxSetObjectPosition(clientID, cube, table,pos, vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(clientID)
def grip_on(): vrep.simxSetObjectParent(clientID, cube, bvp, 1, vrep.simx_opmode_blocking) # vrep.simxSetObjectPosition(clientID, cube, bvp, [0.0,0.0,0.0], vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(clientID)
vrep.simx_opmode_blocking) err, m1 = vrep.simxGetObjectHandle(clientID, "marker1", vrep.simx_opmode_blocking) err, m2 = vrep.simxGetObjectHandle(clientID, "marker2", vrep.simx_opmode_blocking) err, m03 = vrep.simxGetObjectHandle(clientID, "box_center", vrep.simx_opmode_blocking) err, m3 = vrep.simxGetObjectHandle(clientID, "marker3", vrep.simx_opmode_blocking) err, m000 = vrep.simxGetObjectHandle(clientID, "UR5_joint1", vrep.simx_opmode_blocking) err, table = vrep.simxGetObjectHandle(clientID, "diningTable", vrep.simx_opmode_blocking) err, cam = vrep.simxGetObjectHandle(clientID, "sensor_1", vrep.simx_opmode_blocking) vrep.simxSetObjectParent(clientID, cube, table, 1, vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(clientID, cube, m01, [0.0, 0.0, 0.0], vrep.simx_opmode_blocking) jjh = [] for i in range(6): err, jh = vrep.simxGetObjectHandle(clientID, "UR5_joint{}".format(i + 1), vrep.simx_opmode_blocking) jjh += [jh] def get_jj(): jj = [] for i in range(6): err, j = vrep.simxGetJointPosition(clientID, jjh[i], vrep.simx_opmode_blocking)
def drop(clientID, attached_shape): open_gripper(clientID) if attached_shape != None: vrep.simxSetObjectParent(clientID, attached_shape, -1, True, vrep.simx_opmode_blocking)