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)
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)
