def reset(self): self.t = 0.0 # stop last simulation vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_blocking) # start new simulation vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking) [res, self.motor] = vrep.simxGetObjectHandle(self.clientID, 'tubeJoint', vrep.simx_opmode_blocking) [res, self.ball] = vrep.simxGetObjectHandle(self.clientID, 'ball', vrep.simx_opmode_blocking) [res, self.shelf] = vrep.simxGetObjectHandle(self.clientID, 'shelf', vrep.simx_opmode_blocking) #[res,robot] = vrep.simxGetObjectHandle(clientID,'robot',vrep.simx_opmode_blocking) vrep.simxGetObjectOrientation(self.clientID, self.shelf, -1, vrep.simx_opmode_streaming) vrep.simxSynchronousTrigger(self.clientID) vrep.simxSetObjectFloatParameter(self.clientID, self.ball, 3001, 100, vrep.simx_opmode_oneshot) vrep.simxSynchronousTrigger(self.clientID) self.done = False #reset vrep.simxGetObjectPosition(self.clientID, self.ball, -1, vrep.simx_opmode_streaming) return [0, 0]
def restart_simulation(self): self.stop_simulation() self.start_simulation() vrep.simxSetJointPosition(self.clientID, self.jointHandles[0], self.x0[0][0], vrep.simx_opmode_blocking) vrep.simxSetObjectFloatParameter(self.clientID, self.jointHandles[0], 2012, self.x0[0][1], vrep.simx_opmode_blocking) vrep.simxSetObjectPosition(self.clientID, self.tipHandle, -1, self.x0[0][2:5], vrep.simx_opmode_blocking)
def setObjectParameter(clientID, handle, parameter_code, set_val, is_float=True): if is_float: vrep.simxSetObjectFloatParameter(clientID, handle, parameter_code, set_val, oneshot) else: vrep.simxSetObjectIntParameter(clientID, handle, parameter_code, set_val, oneshot)
def setMotorSpeed(clientID, motorHandles, desiredSpd): wheel_radius = 0.63407 * 0.5 # Wheel radius in metre desiredSpd_rps = desiredSpd * (1 / wheel_radius) # m/s into radians per second # print("Desired Speed: " + str(desiredSpd) + " km/hr = " + str(desiredSpd_rps) + " radians per seconds = " + str(math.degrees(desiredSpd_rps)) + "degrees per seconds. = " + str(desiredSpd*(1000/3600)) + "m/s" ) err_code = [] for mHandle in motorHandles: err_code.append(vrep.simxSetJointTargetVelocity(clientID, mHandle, desiredSpd_rps, vrep.simx_opmode_blocking)) vrep.simxSetObjectFloatParameter(clientID, mHandle, vrep.sim_shapefloatparam_init_velocity_g, desiredSpd_rps, vrep.simx_opmode_blocking) return err_code;
def __init__(self): self.dt = 0.001 self.t = 0 self.targetAngle = -1.517 self.motor = None self.ball = None self.shelf = None self.threshold = 0.068 self.low_action = np.array([-1.0]) self.high_action = np.array([1.0]) self.action_space = spaces.Box(low=self.low_action, high=self.high_action, dtype=np.float32) # robot observation space self.observation_space = spaces.Box(low=-np.inf, high=np.inf, shape=(2, ), dtype=np.float32) vrep.simxFinish(-1) # just in case, close all opened connections self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) # Connect to V-REP if self.clientID == -1: print('ERROR: Cannot connected to remote API server') # enable the synchronous mode on the client: vrep.simxSynchronous(self.clientID, True) # start the simulation: vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking) [res, self.motor] = vrep.simxGetObjectHandle(self.clientID, 'tubeJoint', vrep.simx_opmode_blocking) [res, self.ball] = vrep.simxGetObjectHandle(self.clientID, 'ball', vrep.simx_opmode_blocking) [res, self.shelf] = vrep.simxGetObjectHandle(self.clientID, 'shelf', vrep.simx_opmode_blocking) #[res,robot] = vrep.simxGetObjectHandle(clientID,'robot',vrep.simx_opmode_blocking) vrep.simxGetObjectOrientation(self.clientID, self.shelf, -1, vrep.simx_opmode_streaming) vrep.simxSynchronousTrigger(self.clientID) vrep.simxSetObjectFloatParameter(self.clientID, self.ball, 3001, 100, vrep.simx_opmode_oneshot) vrep.simxSynchronousTrigger(self.clientID) self.done = False #reset vrep.simxGetObjectPosition(self.clientID, self.ball, -1, vrep.simx_opmode_streaming)
def setSpringConstant(self, leg="left", k=None, c=None): """leg = 0 left leg, 1 right leg, 2 both legs""" jointfloatparam_kc_k = 2018 jointfloatparam_kc_c = 2019 if k != None: error_k = vrep.simxSetObjectFloatParameter( clientID, self.springHandles[leg], jointfloatparam_kc_k, k, vrep.simx_opmode_oneshot) if c != None: error_c = vrep.simxSetObjectFloatParameter( clientID, self.springHandles[leg], jointfloatparam_kc_c, c, vrep.simx_opmode_oneshot) if error_k or error_c: print("Set Spring Constant Error")
def set_parameter(self, sensor_name, parameter_name, parameter_value): """Set sensor parameters. Sensor name can be fastHokuyo_sensor1, kinect_depth, kinect_rgb. To get list of possible params call youbot.parameters_list""" if parameter_name == 'perspective_angle': parameter_value = parameter_value / (180 * 2) * math.pi if parameter_name in self.params_f: error = vrep.simxSetObjectFloatParameter( self.client_id, self.handles[sensor_name + self.postfix], self.params_f[parameter_name], parameter_value, ONE_SHOT_MODE ) vrep.simxSetFloatSignal( self.client_id, 'change_params', parameter_value, ONE_SHOT_MODE ) vrep.simxClearFloatSignal( self.client_id, 'change_params', ONE_SHOT_MODE ) return error elif parameter_name in self.params_i: error = vrep.simxSetObjectFloatParameter( self.client_id, self.handles[sensor_name + self.postfix], self.params_i[parameter_name], parameter_value, ONE_SHOT_MODE ) vrep.simxSetFloatSignal( self.client_id, 'change_params', parameter_value, ONE_SHOT_MODE ) vrep.simxClearFloatSignal( self.client_id, 'change_params', ONE_SHOT_MODE ) return error else: return 'Parameter not found'
def vrep_change_properties(client_id, object_id, parameter_id, parameter_value): """Changing properties of sensors in vrep client_id (int): ID of current scene in vrep object_id (int): ID of sensor to change parameter_id (int): ID of parameter to change parameter_value (int/double): value of parameter to change """ params_f = { 'near_clipping_plane': 1000, 'far_clipping_plane': 1001, 'perspective_angle': 1004 } params_i = { 'vision_sensor_resolution_x': 1002, 'vision_sensor_resolution_y': 1003 } if parameter_id == 'perspective_angle': parameter_value = parameter_value / (180 * 2) * math.pi if parameter_id in params_f: error = vrep.simxSetObjectFloatParameter(client_id, object_id, params_f[parameter_id], parameter_value, vrep.simx_opmode_blocking) vrep.simxSetFloatSignal(client_id, 'change_params', parameter_value, vrep.simx_opmode_blocking) vrep.simxClearFloatSignal(client_id, 'change_params', vrep.simx_opmode_blocking) return error elif parameter_id in params_i: error = vrep.simxSetObjectIntParameter(client_id, object_id, params_i[parameter_id], parameter_value, vrep.simx_opmode_blocking) vrep.simxSetFloatSignal(client_id, 'change_params', parameter_value, vrep.simx_opmode_blocking) vrep.simxClearFloatSignal(client_id, 'change_params', vrep.simx_opmode_blocking) return error else: return 'parameter wasn\'t found'
def _setup_objects(self): for obj_name, obj_cfg in self.cfg.execute.scene.objects._children_items(): obj_h = self._vrep_get_handle(obj_name) obj_cal = self.caldata.objects[obj_name] if obj_cfg.mass >= 0: MASS_PARAM = 3005 # 3005 is mass param assert remote_api.simxSetObjectFloatParameter(self.api_id, obj_h, MASS_PARAM, obj_cfg.mass, remote_api.simx_opmode_oneshot_wait) == 0 obj_cal = self.caldata.objects[obj_name] obj_pos = obj_cal.actual_pos(obj_cal.pos_w, obj_cfg.pos) self._vrep_set_pos(obj_h, -1, obj_pos) self.objects_pos[obj_name] = obj_pos if obj_cfg.tracked: self.tracked_objects.append(obj_name) self.tracked_handles.append(obj_h)
def _setup_objects(self): for obj_name, obj_cfg in self.cfg.execute.scene.objects._children_items( ): obj_h = self._vrep_get_handle(obj_name) obj_cal = self.caldata.objects[obj_name] if obj_cfg.mass >= 0: MASS_PARAM = 3005 # 3005 is mass param assert remote_api.simxSetObjectFloatParameter( self.api_id, obj_h, MASS_PARAM, obj_cfg.mass, remote_api.simx_opmode_oneshot_wait) == 0 obj_cal = self.caldata.objects[obj_name] obj_pos = obj_cal.actual_pos(obj_cal.pos_w, obj_cfg.pos) self._vrep_set_pos(obj_h, -1, obj_pos) self.objects_pos[obj_name] = obj_pos if obj_cfg.tracked: self.tracked_objects.append(obj_name) self.tracked_handles.append(obj_h)
def set_near_clip_plane(self, clip_plane): """Set near clipping plane.""" if self._handle < 0: if self._handle == MISSING_HANDLE: raise RuntimeError( "Could not set near clipping plane of {}: missing name or " "handle.".format(self._name)) if self._handle == REMOVED_OBJ_HANDLE: raise RuntimeError("Could not set near clipping plane of {}: " "object removed.".format(self._name)) client_id = self.client_id if client_id is None: raise ConnectionError( "Could not set near clipping plane of {}: not connected to " "V-REP remote API server.".format(self._name)) res = vrep.simxSetObjectFloatParameter( client_id, self._handle, vrep.sim_visionfloatparam_near_clipping, clip_plane, vrep.simx_opmode_blocking) if res != vrep.simx_return_ok: raise ServerError("Could not set near clipping plane of {}." "".format(self._name))
vrep.simxPauseSimulation(clientID, vrep.simx_opmode_oneshot_wait) x_req = 1.35 # to be replaced with robot workspace coordinates res1, obj1 = vrep.simxGetObjectHandle(clientID, 'Sphere', vrep.simx_opmode_blocking) if (char == "n"): pose_list = [] xx = [] zz = [] z_vel, x_vel = input("velocity in Z and X: ") time1 = input("Time of flight: ") vrep.simxSetObjectFloatParameter(clientID, obj1, vrep.sim_shapefloatparam_init_velocity_z, z_vel, vrep.simx_opmode_blocking) vrep.simxSetObjectFloatParameter(clientID, obj1, vrep.sim_shapefloatparam_init_velocity_x, x_vel, vrep.simx_opmode_blocking) vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking) t_end = time.time() + time1 #while time.time() < t_end: for i in range(0,10): errorCodeKinectRGB,kinectRGB=vrep.simxGetObjectHandle(clientID,'kinect_rgb',vrep.simx_opmode_blocking) #errorHere,resolution,image=vrep.simxGetVisionSensorImage(clientID,kinectRGB,0,vrep.simx_opmode_blocking) img_time=vrep.simxGetLastCmdTime(clientID) #errorCodeKinectDepth,kinectDepth=vrep.simxGetObjectHandle(clientID,depthSTR,vrep.simx_opmode_blocking) errorHere,resolution,image=vrep.simxGetVisionSensorImage(clientID,kinectRGB,0,vrep.simx_opmode_oneshot_wait) image_byte_array = array.array('b', image)