def initializeVrepApi(self):
# initialize bot handles and variables
_, self.leftMotor=vrep.simxGetObjectHandle(
self.clientID, '%s_leftJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
_, self.rightMotor=vrep.simxGetObjectHandle(
self.clientID, '%s_rightJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
_, self.bot=vrep.simxGetObjectHandle(
self.clientID, self.bot_name, vrep.simx_opmode_oneshot_wait)
# initialize proximity sensors
self.proxSensors = prox_sens_initialize(self.clientID, self.bot_name)
# initialize odom of bot
_, self.xyz = vrep.simxGetObjectPosition(
self.clientID, self.bot, -1, vrep.simx_opmode_streaming )
_, self.eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, self.bot, -1, vrep.simx_opmode_streaming )
# FIXME: striker handles shouldn't be part of the default base class
# FIXME: should be better way to have information regarding all the bots (Central Control?) (Publishers/Subscribers...)?
# initialize bot handles and variables for Red1
_, self.leftMotorStriker=vrep.simxGetObjectHandle(
self.clientID, '%s_leftJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
_, self.rightMotorStriker=vrep.simxGetObjectHandle(
self.clientID, '%s_rightJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
_, self.botStriker = vrep.simxGetObjectHandle(
self.clientID, self.bot_nameStriker, vrep.simx_opmode_oneshot_wait)
_, xyzStriker = vrep.simxGetObjectPosition(
self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming )
_, eulerAnglesStriker = vrep.simxGetObjectOrientation(
self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming )
def robotOrientationToGoal(self):
if self.robotOrientationFirstTime:
error, angle = vrep.simxGetObjectOrientation(self.clientId, self.bioloidHandle, self.cuboidHandle, vrep.simx_opmode_streaming)
self.robotOrientationFirstTime = False
else:
error, angle = vrep.simxGetObjectOrientation(self.clientId, self.bioloidHandle, self.cuboidHandle, vrep.simx_opmode_buffer)
return error, angle[1]
def simGetObjectMatrix(cid,obj,relative):
err, pos = vrep.simxGetObjectPosition(cid,obj,-1,mode())
x,y,z = pos
print(err)
print("Values are {} {} {} {}".format(x,y,z,pos))
err, angles = vrep.simxGetObjectOrientation(cid,obj,-1,mode())
a,b,g = angles
print(err)
print("Angles are {} {} {} {}".format(a,b,g,angles))
op = np.array([[0]*4]*4, dtype =np.float64)
A = float(cos(a))
B = float(sin(a))
C = float(cos(b))
D = float(sin(b))
E = float(cos(g))
F = float(sin(g))
AD = float(A*D)
BD = float(B*D)
op[0][0]=float(C)*E
op[0][1]=-float(C)*F
op[0][2]=float(D)
op[1][0]=float(BD)*E+A*F
op[1][1]=float(-BD)*F+A*E
op[1][2]=float(-B)*C
op[2][0]=float(-AD)*E+B*F
op[2][1]=float(AD)*F+B*E
op[2][2]=float(A)*C
op[0][3]=float(x)
op[1][3]=float(y)
op[2][3]=float(z)
return op[0:3,:]
def get_novel_pose3d():
"""return the pose of the people novel relative to world coordination"""
res, pos_novel = vrep.simxGetObjectPosition(clientID, novelID, -1,
BLOCKING)
res, ori_novel = vrep.simxGetObjectOrientation(clientID, novelID, -1,
BLOCKING)
return pos_novel, ori_novel
def get_robot_pose3d():
"""return the pose of the robot relative to world coordination: """
res, pos_robot_end = vrep.simxGetObjectPosition(clientID, robotEndID, -1,
BLOCKING)
res, ori_robot_end = vrep.simxGetObjectOrientation(clientID, robotEndID,
-1, BLOCKING)
return pos_robot_end, ori_robot_end
def get_orientation(self):
_, orientation = vrep.simxGetObjectOrientation(
self.client_id,
self.handle,
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_blocking)
return orientation
def get_orientation(self, relative=None, prec=None):
"""Retrieve object orientation specified as Euler angles about x, y,
and z axes of the reference frame, each angle between -pi and pi.
"""
if self._handle < 0:
if self._handle == MISSING_HANDLE:
raise RuntimeError(
"Could not retrieve orientation of {}: missing name or "
"handle.".format(self._name))
if self._handle == REMOVED_OBJ_HANDLE:
raise RuntimeError("Could not retrieve orientation of {}: "
"object removed.".format(self._name))
client_id = self.client_id
if client_id is None:
raise ConnectionError(
"Could not retrieve orientation of {}: not connected to V-REP "
"remote API server.".format(self._name))
relative_handle = to_handle(relative, "relative")
res, orientation = vrep.simxGetObjectOrientation(
client_id, self._handle, relative_handle,
vrep.simx_opmode_blocking)
if res != vrep.simx_return_ok:
raise ServerError("Could not retrieve orientation of {}.".format(
self._name))
if prec is None:
return orientation
else:
return [round(angle, prec) for angle in orientation]
def StopSimulator(self):
print('Attempting to Stop the Simulator')
if vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot_wait) != 0:
print('Could not stop the simulator. You can stop the simulator manually by pressing the Stop button in VREP.')
else:
print('Successfully stoped the VREP Simulator.')
# Stop streaming modes to each object
vrep.simxGetObjectPosition(self.clientID, self.robotHandle, -1, vrep.simx_opmode_discontinue)
vrep.simxGetObjectOrientation(self.clientID, self.robotHandle, -1, vrep.simx_opmode_discontinue)
vrep.simxGetObjectPosition(self.clientID, self.cameraHandle, -1, vrep.simx_opmode_discontinue)
vrep.simxGetObjectPosition(self.clientID, self.ballHandle, -1, vrep.simx_opmode_discontinue)
vrep.simxGetObjectPosition(self.clientID, self.blueGoalHandle, -1, vrep.simx_opmode_discontinue)
vrep.simxGetObjectPosition(self.clientID, self.yellowGoalHandle, -1, vrep.simx_opmode_discontinue)
for handle in self.obstacleHandles:
vrep.simxGetObjectPosition(self.clientID, handle, -1, vrep.simx_opmode_discontinue)
def simGetObjectMatrix(cid, obj, relative, firstPass):
err, pos = vrep.simxGetObjectPosition(cid, obj, -1, mode(firstPass))
x, y, z = pos
print(err)
print("Values are {} {} {} {}".format(x, y, z, pos))
err, angles = vrep.simxGetObjectOrientation(cid, obj, -1, mode(firstPass))
a, b, g = angles
print(err)
print("Angles are {} {} {} {}".format(a, b, g, angles))
op = np.array([[0] * 4] * 4, dtype=np.float64)
A = float(cos(a))
B = float(sin(a))
C = float(cos(b))
D = float(sin(b))
E = float(cos(g))
F = float(sin(g))
AD = float(A * D)
BD = float(B * D)
op[0][0] = float(C) * E
op[0][1] = -float(C) * F
op[0][2] = float(D)
op[1][0] = float(BD) * E + A * F
op[1][1] = float(-BD) * F + A * E
op[1][2] = float(-B) * C
op[2][0] = float(-AD) * E + B * F
op[2][1] = float(AD) * F + B * E
op[2][2] = float(A) * C
op[0][3] = float(x)
op[1][3] = float(y)
op[2][3] = float(z)
return op[0:3, :]
def goForRandomRotate(dummy, hand, base, maxVel, maxAccel, maxJerk, ikSteps, emptyBuff, oThreshold):
res,retInts,configBeforeRotate,retStrings,retBuffer = vrep.simxCallScriptFunction(clientID, 'Baxter_rightArm_joint1', vrep.sim_scripttype_childscript, 'pyGetConfig', [], [], [], emptyBuff, vrep.simx_opmode_oneshot_wait)
configAfterRotate = list(configBeforeRotate)
configAfterRotate[6] = random.uniform(-math.pi, math.pi)
moveToConfig(configAfterRotate, maxVel, maxAccel, maxJerk, fkSteps, emptyBuff, oThreshold)
res, handOrientation = vrep.simxGetObjectOrientation(clientID, hand, base, vrep.simx_opmode_blocking)
return handOrientation[2], configBeforeRotate
def getState(self, initial=False):
if initial:
mode = vrep.simx_opmode_streaming
else:
mode = vrep.simx_opmode_buffer
# Retrieve IMU data
errorSignal, self.stepSeconds = vrep.simxGetFloatSignal(self.clientID,
'stepSeconds', mode)
errorOrien, baseEuler = vrep.simxGetObjectOrientation(self.clientID,
self.Quadbase, -1, mode)
errorPos, basePos = vrep.simxGetObjectPosition(self.clientID,
self.Quadbase,-1, mode)
errorVel, linVel, angVel = vrep.simxGetObjectVelocity(self.clientID,
self.Quadbase, mode)
if initial:
if (errorSignal or errorOrien or errorPos or errorVel != vrep.simx_return_ok):
time.sleep(0.05)
pass
else:
# Convert Euler angles to pitch, roll, yaw
rollRad, pitchRad = rotate((baseEuler[0], baseEuler[1]), baseEuler[2])
pitchRad = -pitchRad
yawRad = -baseEuler[2]
baseRad = np.array([yawRad,rollRad,pitchRad])+0.0
self.state = np.asarray(np.concatenate((basePos,linVel,angVel,baseRad)),dtype=np.float32)
#print("data_core: " + str(self.state))
return self.state
def get_goal_pose():
""" return the pose of the robot: [ x(m), y(m), Theta(rad) ] """
global pos
res, pos = vrep.simxGetObjectPosition(clientID,goalID, robotID, MODE)
res, ori = vrep.simxGetObjectOrientation(clientID,goalID, robotID, MODE)
pos = np.array([pos[0], pos[1], pos[2]])
return pos
def _get_observation(self): # get current observations s_t
state = np.zeros(self.obs_dims)
# state is [θ1, θ2, θ3, θ4, θ5, θ6, ω1, ω2, ω3, ω4, ω5, ω6,
# Base_x, Base_y, Base_z, Base_α, Base_β, Base_γ,
# Vx, Vy, Vz, dα, dβ, dγ]
Joint_angle = [vrep.simxGetJointPosition(self.clientID, self.handles['joint'][i],
vrep.simx_opmode_blocking)[1]
for i in range(6)] # retrieve the rotation angle as [θ1, θ2, θ3, θ4, θ5, θ6]
state[0: 6] = Joint_angle
Joint_velocity = [vrep.simxGetObjectFloatParameter(self.clientID, self.handles['joint'][i],
2012, vrep.simx_opmode_blocking)[1]
for i in range(6)] # retrieve the joint velocity as [ω1, ω2, ω3, ω4, ω5, ω6]
state[6: 12] = Joint_velocity
Base_position = [vrep.simxGetObjectPosition(self.clientID, self.handles['base'][0],
-1, vrep.simx_opmode_blocking)[1]]
state[12: 15] = Base_position[0] # retrieve the position of Base as [Base_x, Base_y, Base_z]
Base_pose = [vrep.simxGetObjectOrientation(self.clientID, self.handles['base'][0],
-1, vrep.simx_opmode_blocking)[1]]
state[15: 18] = Base_pose[0] # retrieve the orientation of Base as [Base_α, Base_β, Base_γ]
_, Base_Vel, Base_Ang_Vel = vrep.simxGetObjectVelocity(self.clientID, self.handles['base'][0],
vrep.simx_opmode_blocking)
state[18: 21] = Base_Vel # retrieve the linear velocity of Base as [Vx, Vy, Vz]
state[21: 24] = Base_Ang_Vel # retrieve the angular velocity of Base as [dα, dβ, dγ]
state = np.asarray(state)
return state
def execute_action(clientID, RobotHandle, LMotorHandle, RMotorHandle, raw_pose_data, next_action):
linearVelo, angularVelo, goal_pose = compute_control(raw_pose_data, next_action)
l_ang_v, r_ang_v = dif_drive(linearVelo, angularVelo)
#----------
dist_bound = 0.15
ang_bound = 0.1*PI
# retDia, DialogHandle, uiHandle = vrep.simxDisplayDialog(clientID, 'Current Action', next_action, vrep.sim_dlgstyle_message, 'None', None, None, vrep.simx_opmode_blocking)
#----------
while (distance(raw_pose_data, goal_pose)>= dist_bound) or (abs(raw_pose_data[2]-goal_pose[2])>=ang_bound):
# print '--line_distance--', distance(raw_pose_data, goal_pose)
# print '--angle distance--', abs(raw_pose_data[2]-goal_pose[2])
pret, RobotPos = vrep.simxGetObjectPosition(clientID, RobotHandle, -1, vrep.simx_opmode_blocking)
#print "robot position: (x = " + str(RobotPos[0]) + ", y = " + str(RobotPos[1]) + ")"
oret, RobotOrient = vrep.simxGetObjectOrientation(clientID, RobotHandle, -1, vrep.simx_opmode_blocking)
#print "robot orientation: (a = " + str(RobotOrient[0]) + ", b = " + str(RobotOrient[1]) +", g = " + str(RobotOrient[2]) + ")"
raw_pose_data = shift_raw_pose([RobotPos[0], RobotPos[1], RobotOrient[2]])
#------------------------------
linearVelo, angularVelo = Find_Control(raw_pose_data, next_action, goal_pose)
l_ang_v, r_ang_v = dif_drive(linearVelo, angularVelo)
#print 'raw_pose_data', raw_pose_data
vrep.simxSetJointTargetVelocity(clientID, LMotorHandle, l_ang_v, vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, RMotorHandle, r_ang_v, vrep.simx_opmode_streaming)
#print 'distance_x_y:%s; angle_dif:%s' %(str(distance(raw_pose_data, goal_pose)),str(abs(raw_pose_data[2]-goal_pose[2])))
print 'Goal pose reached!'
# vrep.simxSetJointTargetVelocity(clientID, LMotorHandle, 0, vrep.simx_opmode_blocking)
# vrep.simxSetJointTargetVelocity(clientID, RMotorHandle, 0, vrep.simx_opmode_blocking)
# time.sleep(0.5)
# retEnd = vrep.simxEndDialog(clientID, DialogHandle, vrep.simx_opmode_blocking)
return raw_pose_data
def pre_read(self):
[r, no] = vrep.simxGetObjectPosition(self.clientID, self.body, -1,
vrep.simx_opmode_streaming) # 读位置
[r, useless
] = vrep.simxGetObjectOrientation(self.clientID, self.body, -1,
vrep.simx_opmode_streaming) # 读角度
time.sleep(0.05) #要等待片刻
def update_dynamic_obj_info(self):
for dynamic_obj in (self.dynamic_obj_list + self.robot_obj_list):
obj_handle = dynamic_obj.handle
# Get pose and orientation
returnCode, obj_pose = vrep.simxGetObjectPosition(self.clientID, obj_handle, -1, self.operation_mode)
returnCode, obj_ori = vrep.simxGetObjectOrientation(self.clientID, obj_handle, -1, self.operation_mode)
# Get the velocity of the obstacle
returnCode, param_vel_x = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 11, self.operation_mode)
returnCode, param_vel_y = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 12, self.operation_mode)
returnCode, param_vel_z = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 13, self.operation_mode)
returnCode, param_vel_r = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 14, self.operation_mode)
obj_vel = np.array([param_vel_x, param_vel_y, param_vel_z, param_vel_r])
obj_size = dynamic_obj.size
bbox_min = [obj_pose[0]-obj_size[0]/2.0, obj_pose[1]-obj_size[1]/2.0, obj_pose[2]-obj_size[2]/2.0]
bbox_max = [obj_pose[0]+obj_size[0]/2.0, obj_pose[1]+obj_size[1]/2.0, obj_pose[2]+obj_size[2]/2.0]
# TODO: must check if obj is really updating
dynamic_obj.pose = obj_pose
dynamic_obj.ori = obj_ori
dynamic_obj.vel = obj_vel
dynamic_obj.bbox_min = bbox_min
dynamic_obj.bbox_max = bbox_max
def wait_till_stream(self):
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, ori = vrep.simxGetObjectOrientation(self.client_id, self.body_handle, -1, vrep.simx_opmode_buffer)
self.logger.info("orient:%s" % str(ori))
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetObjectPosition(self.client_id, self.body_handle, -1, vrep.simx_opmode_buffer)
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetJointPosition(self.client_id, self.left_joint, vrep.simx_opmode_buffer)
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetJointPosition(self.client_id, self.right_joint, vrep.simx_opmode_buffer)
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetObjectFloatParameter(self.client_id, self.right_joint, 2012, vrep.simx_opmode_buffer)
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetObjectFloatParameter(self.client_id, self.right_joint, 2012, vrep.simx_opmode_buffer)
def run(self):
global left_wheel_angle, right_wheel_angle, pos_x, pos_y, angle
_, position = vrep.simxGetObjectPosition(self.clientID,
self.robot_handle, -1,
vrep.simx_opmode_buffer)
_, eulerAngles = vrep.simxGetObjectOrientation(self.clientID,
self.robot_handle, -1,
vrep.simx_opmode_buffer)
_, new_right_wheel_angle = vrep.simxGetJointPosition(
self.clientID, self.right_motor_handle, vrep.simx_opmode_streaming)
_, new_left_wheel_angle = vrep.simxGetJointPosition(
self.clientID, self.left_motor_handle, vrep.simx_opmode_streaming)
pos_x = position[0]
pos_y = position[1]
angle = eulerAngles[2]
new_left_wheel_angle = (new_left_wheel_angle + 2 * PI) % (2 * PI)
new_right_wheel_angle = (new_right_wheel_angle + 2 * PI) % (2 * PI)
left_wheel_angle = new_left_wheel_angle
right_wheel_angle = new_right_wheel_angle
while True:
self.update()
time.sleep(0.1)
def _setup_sim_camera(self):
"""
-- Get some param and handles from the simulation scene
and set necessary parameter for camera
"""
# Get handle to camera
_, self.cam_handle = vrep.simxGetObjectHandle(
self.clientID, self.Camera_NAME, vrep.simx_opmode_oneshot_wait)
_, self.kinectRGB_handle = vrep.simxGetObjectHandle(
self.clientID, self.Camera_RGB_NAME, vrep.simx_opmode_oneshot_wait)
_, self.kinectDepth_handle = vrep.simxGetObjectHandle(
self.clientID, self.Camera_DEPTH_NAME,
vrep.simx_opmode_oneshot_wait)
# Get camera pose and intrinsics in simulation
_, self.cam_position = vrep.simxGetObjectPosition(
self.clientID, self.cam_handle, -1, vrep.simx_opmode_oneshot_wait)
_, cam_orientation = vrep.simxGetObjectOrientation(
self.clientID, self.cam_handle, -1, vrep.simx_opmode_oneshot_wait)
self.cam_trans = np.eye(4, 4)
self.cam_trans[0:3, 3] = np.asarray(self.cam_position)
self.cam_orientation = [
-cam_orientation[0], -cam_orientation[1], -cam_orientation[2]
]
self.cam_rotm = np.eye(4, 4)
self.cam_rotm[0:3,
0:3] = np.linalg.inv(self._euler2rotm(cam_orientation))
self.cam_pose = np.dot(
self.cam_trans, self.cam_rotm
) # Compute rigid transformation representating camera pose
self._intri_camera()
def get_orientation(self, name):
""" Returns the orientation of an object in VREP
the Euler angles [radians] are returned in the relative xyz frame.
http://www.coppeliarobotics.com/helpFiles/en/eulerAngles.htm
Parameters
----------
name : string
the name of the object of interest
"""
if self.misc_handles.get(name, None) is None:
# if we haven't retrieved the handle previously
# get the handle and set up streaming
_, self.misc_handles[name] = \
vrep.simxGetObjectHandle(self.clientID,
name,
vrep.simx_opmode_blocking)
_, orientation = \
vrep.simxGetObjectOrientation(
self.clientID,
self.misc_handles[name],
-1, # orientation relative to world
vrep.simx_opmode_blocking)
return orientation
def getState(self, initial=False):
if initial:
mode = vrep.simx_opmode_streaming
else:
mode = vrep.simx_opmode_buffer
# Retrieve IMU data
errorSignal, self.stepSeconds = vrep.simxGetFloatSignal(
self.clientID, 'stepSeconds', mode)
errorOrien, baseEuler = vrep.simxGetObjectOrientation(
self.clientID, self.Quadbase, -1, mode)
errorPos, basePos = vrep.simxGetObjectPosition(self.clientID,
self.Quadbase, -1, mode)
errorVel, linVel, angVel = vrep.simxGetObjectVelocity(
self.clientID, self.Quadbase, mode)
if initial:
if (errorSignal or errorOrien or errorPos
or errorVel != vrep.simx_return_ok):
time.sleep(0.05)
pass
else:
# Convert Euler angles to pitch, roll, yaw
rollRad, pitchRad = rotate((baseEuler[0], baseEuler[1]),
baseEuler[2])
pitchRad = -pitchRad
yawRad = -baseEuler[2]
baseRad = np.array([yawRad, rollRad, pitchRad]) + 0.0
self.state = np.asarray(np.concatenate(
(basePos, linVel, angVel, baseRad)),
dtype=np.float32)
#print("data_core: " + str(self.state))
return self.state
def readPositionSensors(self):
"""
get the position of robot TODO
"""
# get position
error, position_hexa_base = vrep.simxGetObjectPosition(
clientID, self.positionSensors["body"], -1,
vrep.simx_opmode_blocking)
time = vrep.simxGetLastCmdTime(clientID)
if error == 0:
self.XYZdata('bodyPosition', position_hexa_base, time)
else:
print("Position sensor read error: %d", error)
# get orientation
error, orientation_hexa_base = vrep.simxGetObjectOrientation(
clientID, self.positionSensors["body"], -1,
vrep.simx_opmode_blocking)
time = vrep.simxGetLastCmdTime(clientID)
if error == 0:
self.XYZdata('bodyOrientation', orientation_hexa_base, time)
else:
print("Orientation sensor read error: %d", error)
############################################################################
error, position_hexa = vrep.simxGetObjectPosition(
clientID, self.positionSensors["gyro"], -1,
vrep.simx_opmode_blocking)
time = vrep.simxGetLastCmdTime(clientID)
if error == 0:
self.XYZdata('gyro', position_hexa, time)
else:
print("Gyro sensor read error: %d", error)
def _init_values(self):
error_code, _ = vrep.simxGetObjectPosition(self.client_id, self.target_handle, -1, vrep.simx_opmode_oneshot)
error_code, _ = vrep.simxGetObjectPosition(self.client_id, self.bot_handle, -1, vrep.simx_opmode_oneshot)
error_code, _ = vrep.simxGetObjectOrientation(self.client_id, self.bot_handle, -1, vrep.simx_opmode_streaming)
def get_pose():
errorcode, pos = vrep.simxGetObjectPosition(clientID, robotID, -1,
vrep.simx_opmode_buffer)
errorcode, ori = vrep.simxGetObjectOrientation(clientID, robotID, -1,
vrep.simx_opmode_buffer)
pos = np.array([pos[0], pos[1], ori[2]])
return pos
def get_dynamic_obj_info(self):
dynamic_obj_list = []
for obj_name in self.dynamic_obj_name_list:
#returnCode, obj_handle = vrep.simxGetObjectHandle(self.clientID, obj_name, self.operation_mode)
returnCode, obj_handle = vrep.simxGetObjectHandle(
self.clientID, obj_name, vrep.simx_opmode_oneshot_wait)
# Skip non-existent objects
if (returnCode != 0):
continue
# Get pose and orientation
returnCode, obj_pose = vrep.simxGetObjectPosition(
self.clientID, obj_handle, -1, self.operation_mode)
returnCode, obj_ori = vrep.simxGetObjectOrientation(
self.clientID, obj_handle, -1, self.operation_mode)
# Get the velocity of the obstacle
returnCode, param_vel_x = vrep.simxGetObjectFloatParameter(
self.clientID, obj_handle, 11, self.operation_mode)
returnCode, param_vel_y = vrep.simxGetObjectFloatParameter(
self.clientID, obj_handle, 12, self.operation_mode)
returnCode, param_vel_z = vrep.simxGetObjectFloatParameter(
self.clientID, obj_handle, 13, self.operation_mode)
returnCode, param_vel_r = vrep.simxGetObjectFloatParameter(
self.clientID, obj_handle, 14, self.operation_mode)
obj_vel = np.array(
[param_vel_x, param_vel_y, param_vel_z, param_vel_r])
self.dynamic_obj_list = dynamic_obj_list
def reset(self):
super(DishRackEnv, self).reset()
self.rack_pos[0] = self.np_random.uniform(rack_lower[0], rack_upper[0])
self.rack_pos[1] = self.np_random.uniform(rack_lower[1], rack_upper[1])
self.rack_rot[0] = self.np_random.uniform(rack_lower[2], rack_upper[2])
vrep.simxSetObjectPosition(self.cid, self.rack_handle, -1,
self.rack_pos, vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(self.cid, self.rack_handle,
self.rack_rot_ref, self.rack_rot,
vrep.simx_opmode_blocking)
trg_rot = catch_errors(
vrep.simxGetObjectOrientation(self.cid, self.target_handle, -1,
vrep.simx_opmode_blocking))
vrep.simxSetObjectOrientation(self.cid, self.mv_trg_handle, -1,
trg_rot, vrep.simx_opmode_blocking)
if self.vis_mode:
# OTHER POSES
stand_height_diff = np.append([0, 0],
self.np_random.uniform(
-self.max_height_displacement,
self.max_height_displacement, 1))
vrep.simxSetObjectPosition(self.cid, self.stand_h, -1,
self.init_stand_pos + stand_height_diff,
vrep.simx_opmode_blocking)
self.randomise_domain()
return self._get_obs()
def __init__(self, clientid):
self.clientID = clientid
self.accelData = (0, 0, 0)
self.gyroData = (0, 0, 0)
self.sensErr = 0.04
res, self.accelHandle = vrep.simxGetObjectHandle(
self.clientID, 'Accelerometer', vrep.simx_opmode_blocking)
res, self.gyroHandle = vrep.simxGetObjectHandle(
self.clientID, 'GyroSensor', vrep.simx_opmode_blocking)
res, self.magnHandle = vrep.simxGetObjectHandle(
self.clientID, 'Magnetometr', vrep.simx_opmode_blocking)
err, accelX = vrep.simxGetFloatSignal(self.clientID, 'accelerometerX',
vrep.simx_opmode_streaming)
err, accelY = vrep.simxGetFloatSignal(self.clientID, 'accelerometerY',
vrep.simx_opmode_streaming)
err, accelZ = vrep.simxGetFloatSignal(self.clientID, 'accelerometerZ',
vrep.simx_opmode_streaming)
err, gyroX = vrep.simxGetFloatSignal(self.clientID, 'gyroX',
vrep.simx_opmode_streaming)
err, gyroY = vrep.simxGetFloatSignal(self.clientID, 'gyroY',
vrep.simx_opmode_streaming)
err, gyroZ = vrep.simxGetFloatSignal(self.clientID, 'gyroZ',
vrep.simx_opmode_streaming)
res, euler = vrep.simxGetObjectOrientation(self.clientID,
self.magnHandle, 0,
vrep.simx_opmode_streaming)
def getVehicleState(): global clientID, arm_joint, mobile_joint, joint_handles, gripper_handles, throttle_handles, body_handle, pubOdom, Pose, EKF, elapsedTime ret, xyz=vrep.simxGetObjectPosition(clientID, body_handle[1], -1, vrep.simx_opmode_blocking) Vxyz=vrep.simxGetObjectVelocity(clientID, body_handle[1], vrep.simx_opmode_blocking) ret, rot=vrep.simxGetObjectOrientation(clientID, body_handle[1], -1, vrep.simx_opmode_blocking) position=np.array(xyz) orientation=np.array(transforms3d.euler.euler2quat(rot[0],rot[1],rot[2])) velocity=Vxyz[1] angularVelocity=Vxyz[2] odom.header.stamp.secs=int(elapsedTime) odom.header.stamp.nsecs=int((elapsedTime-int(elapsedTime))*1e9) odom.pose.pose.position.x=position[0] odom.pose.pose.position.y=position[1] odom.pose.pose.position.z=position[2] odom.pose.pose.orientation.w=orientation[0] odom.pose.pose.orientation.x=orientation[1] odom.pose.pose.orientation.y=orientation[2] odom.pose.pose.orientation.z=orientation[3] odom.twist.twist.linear.x=velocity[0] odom.twist.twist.linear.y=velocity[1] odom.twist.twist.linear.z=velocity[2] odom.twist.twist.angular.x=angularVelocity[0] odom.twist.twist.angular.y=angularVelocity[1] odom.twist.twist.angular.z=angularVelocity[2] pubOdom.publish(odom)
def stream_head_frame_pose(clientID, head_frame_handle):
rc_head_posi, head_posi = vrep.simxGetObjectPosition(clientID, head_frame_handle, -1, vrep.simx_opmode_buffer)
rc_head_ori, head_ori = vrep.simxGetObjectOrientation(clientID, head_frame_handle, -1, vrep.simx_opmode_buffer)
head_frame_pose = np.array([[cos(head_ori[-1]), -sin(head_ori[-1]), head_posi[0]],
[sin(head_ori[-1]), cos(head_ori[-1]), head_posi[1]],
[0, 0, 1]])
return head_frame_pose
def go_str2(cl_h, pl_h, l_j, r_j, velocity, distance):
direction = 1
cur_angle = sim.simxGetObjectOrientation(cl_h, pl_h, -1,
sim.simx_opmode_blocking)[1]
if 1 < math.fabs(cur_angle[2]) < 2:
direction = 0
old_pos = sim.simxGetObjectPosition(cl_h, pl_h, -1,
sim.simx_opmode_blocking)[1]
disAmount = 0
vel_amount(cl_h, l_j, r_j, velocity)
while 1:
pos = sim.simxGetObjectPosition(cl_h, pl_h, -1,
sim.simx_opmode_blocking)[1]
d = pos[direction] - old_pos[direction]
disAmount = disAmount + math.fabs(d)
if 1.2 * disAmount > distance * 0.5:
vel_amount(cl_h, l_j, r_j, velocity / 4)
if 1.1 * disAmount > distance * 0.5:
vel_amount(cl_h, l_j, r_j, velocity / 6)
if math.fabs((pos[direction] + 0.25) % 0.5) < 0.015:
vel_amount(cl_h, l_j, r_j, 0)
return
else:
vel_amount(cl_h, l_j, r_j, velocity)
old_pos = pos
def step_check_routine(clientID, cup_handle, end_effector_handle,
particle_handle, distance_threshold):
'''
end_effector_orientation_vector : the normal vector of the end_effector in 3d
'''
step_spillage = 0
returnCode, cup_position = vrep.simxGetObjectPosition(
clientID, cup_handle, -1, vrep.simx_opmode_buffer)
returnCode, end_effector_euler_angles = vrep.simxGetObjectOrientation(
clientID, end_effector_handle, -1, vrep.simx_opmode_buffer)
returnCode, end_effector_position = vrep.simxGetObjectPosition(
clientID, end_effector_handle, -1, vrep.simx_opmode_buffer)
# Post-processing
if returnCode != vrep.simx_return_ok:
print('Can not find Cup positions!')
# ------------------------------------compute the particles in source container-----------------------------------
for i in range(len(particle_handle)):
returnCode, particle_position = vrep.simxGetObjectPosition(
clientID, particle_handle[i], -1, vrep.simx_opmode_buffer)
if returnCode != vrep.simx_return_ok:
print('cannot get particle position.')
elif np.linalg.norm(
np.array(particle_position) -
np.array(cup_position)) >= distance_threshold:
step_spillage += 1
return step_spillage, np.array(
end_effector_position), end_effector_euler_angles
def reset(self):
self.setting_params_for_rl()
self.setting_params_for_straight()
self.setting_params_for_purepersuit()
[r,
self.pet] = vrep.simxGetObjectPosition(self.clientID, self.body, -1,
vrep.simx_opmode_streaming)
[r,
self.po] = vrep.simxGetObjectOrientation(self.clientID, self.body, -1,
vrep.simx_opmode_streaming)
self.xianx = self.pet[0]
self.xiany = self.pet[1]
self.juli = ((self.xianx - self.tarx[self.stepp])**2 +
(self.xiany - self.tary[self.stepp])**2)**0.5
# 与跟终点距离
self.distance = self.juli
self.xiu = angles.cielv(self.tarx[self.stepp] - self.xianx,
self.tary[self.stepp] - self.xiany)
self.bno = angles.zhuanj(self.po)
# 用于强化学习的reward和state
self.X = angles.jiaocha(self.bno, self.tarj[self.stepp])
self.ey = math.fabs(self.juli * math.sin(
(self.tarj[self.stepp] - self.xiu) / 180 * math.pi))
return [self.X, self.dX, self.ey, self.dey]
def reset():
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
time.sleep(0.1)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
err, pos = vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
err, euler = vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_buffer)
return np.array(euler)
def pure_persuit(self):
if self.yt % 5 == 0:
[r,
self.pet] = vrep.simxGetObjectPosition(self.clientID, self.body,
-1,
vrep.simx_opmode_streaming)
[r, self.po
] = vrep.simxGetObjectOrientation(self.clientID, self.body, -1,
vrep.simx_opmode_streaming)
self.xianx = self.pet[0]
self.xiany = self.pet[1]
if (self.stepp == self.pointnum):
print("ended")
self.done = True
ret = vrep.simxStopSimulation(self.clientID,
vrep.simx_opmode_blocking)
while ret:
ret = vrep.simxStopSimulation(self.clientID,
vrep.simx_opmode_blocking)
return
self.juli = ((self.xianx - self.tarx[self.stepp])**2 +
(self.xiany - self.tary[self.stepp])**2)**0.5
# 与跟踪点距离
if self.juli > 2 * self.distance:
print("out of distance ended")
self.done = True
ret = vrep.simxStopSimulation(self.clientID,
vrep.simx_opmode_blocking)
while ret:
ret = vrep.simxStopSimulation(self.clientID,
vrep.simx_opmode_blocking)
return
self.xiu = angles.cielv(self.tarx[self.stepp] - self.xianx,
self.tary[self.stepp] - self.xiany)
self.bno = angles.zhuanj(self.po)
#用于强化学习的reward和state
self.X = angles.jiaocha(self.bno, self.tarj[self.stepp])
self.ey = math.fabs(self.juli * math.sin(
(self.tarj[self.stepp] - self.xiu) / 180 * math.pi))
#ey为大于等于0的数
print("X", self.X, "ey", self.ey)
# 仿真每一步是0.05秒，每五次控制一步，即0.25秒
self.dX = (self.X - self.prev_X) / 0.25
self.dey = (self.X - self.prev_ey) / 0.25
#更新这两个参量
self.prev_X = self.X
self.prev_ey = self.ey
#reward结束
if self.juli < 0.05: #到达跟踪点
self.juliq = 2147483641
self.stepp = self.stepp + 1 #换下一个跟踪点
self.distance = ((self.xianx - self.tarx[self.stepp])**2 +
(self.xiany - self.tary[self.stepp])**2)**0.5
else:
self.juliq = self.juli
def get_robot_pose2d():
""" return the pose of the robot relative to world coordination: [ x(m), y(m), Theta(rad) ] """
global pos_absolute
res, pos = vrep.simxGetObjectPosition(clientID, robotID, -1, MODE)
res, ori = vrep.simxGetObjectOrientation(clientID, robotID, -1, MODE)
pos_absolute = np.array([pos[0], pos[1], ori[2]])
return pos_absolute
def loop(self):
operationMode=vrep.simx_opmode_streaming
if self.__initLoop:
self.__initLoop=False
else:
operationMode=vrep.simx_opmode_buffer
returnCode, orientation = vrep.simxGetObjectOrientation(self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode==vrep.simx_return_ok:
self.__orientation=orientation[2]
else:
self.__orientation = None
# print >> sys.stderr, "Error in VRepBubbleRob.getOrientation()"
self.__mind.setInput("orientation", self.__orientation)
returnCode, position = vrep.simxGetObjectPosition(self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode==vrep.simx_return_ok:
self.__position=[0.0,0.0]
self.__position[0]=position[0] #X
self.__position[1]=position[1] #Y
else:
self.__position=None
# print >> sys.stderr, "Error in VRepBubbleRob.getPosition()"
returnCode, linearVelocity, angularVelocity = vrep.simxGetObjectVelocity(self.__clientID, self.__bodyHandle, operationMode)
if returnCode==vrep.simx_return_ok:
try:
# self.__velocity=linearVelocity[0]*math.cos(self.__orientation)+linearVelocity[1]*math.sin(self.__orientation)
self.__velocity=math.sqrt(linearVelocity[0]**2+linearVelocity[1]**2)
self.__mind.setInput("velocity", self.__velocity)
except TypeError:
pass
# if self.__name=="BubbleRob#1":
# print self.__velocity, linearVelocity[0], math.cos(self.__orientation), linearVelocity[1], math.sin(self.__orientation)
else:
self.__velocity=None
# print >> sys.stderr, "Error in VRepBubbleRob.getPosition()"
returnCode, sensorTrigger, dp, doh, dsnv = vrep.simxReadProximitySensor(self.__clientID, self.__sensorHandle, operationMode)
if returnCode==vrep.simx_return_ok:
# We succeeded at reading the proximity sensor
self.__mind.setInput("lastProximitySensorTime", vrep.simxGetLastCmdTime(self.__clientID))
self.__mind.setInput("sensorTrigger", sensorTrigger)
self.__mind.setInput("mostSalientItem", self.__mind.selectMostSalient("I"))
# print self.__name, self.__mind.getAttendedItem(self.__mind.getOutput("attendedItemName"))
self.__mind.setInput("attendedItem", self.__mind.getAttendedItem(self.__mind.getOutput("attendedItemName")))
self.__mind.applyRules()
self.__mind.setStates()
if self.__mind.getOutput("steering")!=None:
self.setSteering(self.__mind.getOutput("steering"))
if self.__mind.getOutput("thrust")!=None:
self.setThrust(self.__mind.getOutput("thrust"))
if self.__mind.getOutput("reward")!=None:
if self.__mind.getOutput("reward")>0.5:
self.setEmotionalExpression(1)
elif self.__mind.getOutput("reward")<-0.5:
self.setEmotionalExpression(-1)
else:
self.setEmotionalExpression(0)
getSignalReturnCode, dMessage = vrep.simxGetStringSignal(self.__clientID, "Debug", vrep.simx_opmode_streaming)
if dMessage!="":
print("Debug:"+dMessage)
self.__cnt=self.__cnt+1
def get_target( self ):
err, self.t_ori = vrep.simxGetObjectOrientation(self.cid, self.target, -1,
vrep_mode )
err, self.t_pos = vrep.simxGetObjectPosition(self.cid, self.target, -1,
vrep_mode )
# Convert orientations to z-y-x convention
self.t_ori = convert_angles(self.t_ori)
def getDif(cid,copter,target):
copter_pos = vrep.simxGetObjectPosition(cid, copter, -1, mode())[1]
copter_vel = vrep.simxGetObjectVelocity(cid, copter, mode())[1]
copter_orientation = vrep.simxGetObjectOrientation(cid,copter,-1,mode())[1]
target_pos = vrep.simxGetObjectPosition(cid, target, -1, mode())[1]
target_vel = vrep.simxGetObjectVelocity(cid, target, mode())[1]
return np.asarray([(-np.asarray(copter_pos) + np.asarray(target_pos)),(-np.asarray(copter_vel) + np.asarray(target_vel)),np.asarray(copter_orientation)]).flatten()
def get_target(self):
time.sleep(0.1)
ret, xyz =vrep.simxGetObjectPosition(self.clientID, self.target, -1,vrep.simx_opmode_oneshot)
print(ret, xyz)
x,y,z = xyz
ret, orientation = vrep.simxGetObjectOrientation(self.clientID, self.target,-1, vrep.simx_opmode_oneshot)
rot = orientation[2]
return (x, y, z, rot)
def getRobotPose(self, robot_handle):
_, xyz = vrep.simxGetObjectPosition(
self.clientID, robot_handle, -1, vrep.simx_opmode_buffer)
_, eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, robot_handle, -1, vrep.simx_opmode_buffer)
x, y, z = xyz
theta = eulerAngles[2]
return (x, y, theta)
def read_values(self):
error_code, target_pos = vrep.simxGetObjectPosition(self.client_id, self.target_handle, -1, vrep.simx_opmode_streaming)
self.target_pos = Vector3(x=target_pos[0], y=target_pos[1], z=target_pos[2])
error_code, bot_pos = vrep.simxGetObjectPosition(self.client_id, self.bot_handle, -1, vrep.simx_opmode_streaming)
self.bot_pos = Vector3(x=bot_pos[0], y=bot_pos[1], z=bot_pos[2])
error_code, bot_euler_angles = vrep.simxGetObjectOrientation(self.client_id, self.bot_handle, -1, vrep.simx_opmode_streaming)
self.bot_euler_angles = Vector3(x=bot_euler_angles[0], y=bot_euler_angles[1], z=bot_euler_angles[2])
def robot_pose_get(self):
_, xyz = vrep.simxGetObjectPosition(
self.clientID, self.ePuck, -1, vrep.simx_opmode_buffer)
_, eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, self.ePuck, -1, vrep.simx_opmode_buffer)
x, y, z = xyz
theta = eulerAngles[2]
self.theta_history.append(theta)
return (x, y, theta)
def printRobotLocation(self):
"print the position and orientation of the robot (x, y, yaw)"
errorCode, angle=vrep.simxGetObjectOrientation(self.clientID,self.robot,-1, vrep.simx_opmode_streaming)
errorCode, position=vrep.simxGetObjectPosition(self.clientID,self.robot,-1, vrep.simx_opmode_streaming)
yaw='%.2f'%(angle[2]*180/3.14159265)
x='%.2f'%(position[0])
y='%.2f'%(position[1])
print "Position: (", x,",", y, ")", "\nYaw:", yaw
def get_object_pose(self, name):
"""Get an (x,y,theta) pose for object with name <name>"""
handle = self.get_handle(name)
if handle not in self._p:
vrep.simxGetObjectPosition(
self.id, handle, -1, vrep.simx_opmode_streaming)
vrep.simxGetObjectOrientation(
self.id, handle, -1, vrep.simx_opmode_streaming)
self._p.append(handle)
time.sleep(0.5)
r, position = vrep.simxGetObjectPosition(
self.id, handle, -1, vrep.simx_opmode_buffer)
if r != vrep.simx_return_ok:
raise NameError('Position unknown')
r, orientation = vrep.simxGetObjectOrientation(
self.id, handle, -1, vrep.simx_opmode_buffer)
if r != vrep.simx_return_ok:
raise NameError('Orientation unknown')
return (position[0], position[1], orientation[2])
def initializeVrepApi(self):
# initialize bot handles and variables
_, self.leftMotor=vrep.simxGetObjectHandle(
self.clientID, '%s_leftJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
_, self.rightMotor=vrep.simxGetObjectHandle(
self.clientID, '%s_rightJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
_, self.bot=vrep.simxGetObjectHandle(
self.clientID, self.bot_name, vrep.simx_opmode_oneshot_wait)
# proxSens = prox_sens_initialize(self.clientID)
# initialize odom of bot
_, self.xyz = vrep.simxGetObjectPosition(
self.clientID, self.bot, -1, vrep.simx_opmode_streaming)
_, self.eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, self.bot, -1, vrep.simx_opmode_streaming)
# # initialize overhead cam
# _, self.overheadCam=vrep.simxGetObjectHandle(
# self.clientID, 'Global_Vision', vrep.simx_opmode_oneshot_wait)
# _, self.resolution, self.image = vrep.simxGetVisionSensorImage(
# self.clientID,self.overheadCam,0,vrep.simx_opmode_oneshot_wait)
# # initialize goal handle + odom
# _, self.goalHandle=vrep.simxGetObjectHandle(
# self.clientID, 'Goal_Position', vrep.simx_opmode_oneshot_wait)
# _, self.goalPose = vrep.simxGetObjectPosition(
# self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming)
# initialize bot handles and variables for Red1
_, self.leftMotorStriker=vrep.simxGetObjectHandle(
self.clientID, '%s_leftJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
_, self.rightMotorStriker=vrep.simxGetObjectHandle(
self.clientID, '%s_rightJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
_, self.botStriker = vrep.simxGetObjectHandle(
self.clientID, self.bot_nameStriker, vrep.simx_opmode_oneshot_wait)
_, xyzStriker = vrep.simxGetObjectPosition(
self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming)
_, eulerAnglesStriker = vrep.simxGetObjectOrientation(
self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming)
def runPioneerToAngle(goto_angle):
abs_angle_all = vrep.simxGetObjectOrientation(clientID, pioneerRobotHandle, -1, vrep.simx_opmode_oneshot)
abs_angle = abs_angle_all[1][2]
diff = goto_angle - abs_angle
#diff = math.fabs(diff)
if 0 <= diff and diff < math.pi :
DIRECTION = MOTOR_LEFT
elif math.pi <= diff :
DIRECTION = MOTOR_RIGHT
elif math.pi*(-1) <= diff and diff < 0 :
DIRECTION = MOTOR_RIGHT
elif diff < math.pi*(-1):
DIRECTION = MOTOR_LEFT
while abs_angle > (goto_angle + hysteres) or abs_angle < (goto_angle - hysteres):
run_motor(DIRECTION, MID_SPEED, SHORT_TIME, 0)
abs_angle_all = vrep.simxGetObjectOrientation(clientID, pioneerRobotHandle, -1, vrep.simx_opmode_oneshot)
abs_angle = abs_angle_all[1][2]
def get_position(self):
"""Get the position (x,y,theta) of the robot
Return:
position (list): the position [x,y,theta]
"""
position = []
res, tmp = vrep.simxGetObjectPosition(self.client_id, self.pioneer, -1, vrep.simx_opmode_oneshot_wait)
position.append(tmp[0])
position.append(tmp[1])
res, tmp = vrep.simxGetObjectOrientation(self.client_id, self.pioneer, -1, vrep.simx_opmode_oneshot_wait)
position.append(tmp[2]) # en radian
return position
def runFromObstacle(sensorIndex, distArray):
global recursion_counter
abs_angle = vrep.simxGetObjectOrientation(clientID, pioneerRobotHandle, -1, vrep.simx_opmode_oneshot)
if distArray[sensorIndex][1] == USS_LF_4:
angle = convertAngle(abs_angle[1][2] - math.pi/2 - 0.1) #0.1 less than the turn in the other direction so the robot wont stuck.
elif distArray[sensorIndex][1] == USS_LF_3:
angle = convertAngle(abs_angle[1][2] - 2)
elif distArray[sensorIndex][1] == USS_LF_2:
angle = convertAngle(abs_angle[1][2] - 2.5)
elif distArray[sensorIndex][1] == USS_LF_1:
angle = convertAngle(abs_angle[1][2] - 2.5)
elif distArray[sensorIndex][1] == USS_RF_1:
angle = convertAngle(abs_angle[1][2] + 2.5)
elif distArray[sensorIndex][1] == USS_RF_2:
angle = convertAngle(abs_angle[1][2] + 2.5)
elif distArray[sensorIndex][1] == USS_RF_3:
angle = convertAngle(abs_angle[1][2] + 2)
elif distArray[sensorIndex][1] == USS_RF_4:
angle = convertAngle(abs_angle[1][2] + math.pi/2)
runPioneerToAngle(angle)
distArray = updateSensors()
if avoidObstacles(distArray) == False :
recursion_counter = 0
run_motor(MOTOR_FRONT, MID_SPEED, RUN_FRONT_TIME, 0)
else :
recursion_counter = recursion_counter + 1
if recursion_counter > 50 :
recursion_counter = 0
run_random()
def calculate_error( self ):
# Return the state variables
err, self.ori = vrep.simxGetObjectOrientation(self.cid, self.copter, -1,
vrep_mode )
err, self.pos = vrep.simxGetObjectPosition(self.cid, self.copter, -1,
vrep_mode )
err, self.lin, self.ang = vrep.simxGetObjectVelocity(self.cid, self.copter,
vrep_mode )
self.ori = convert_angles(self.ori)
# Apply noise to each measurement if required
#FIXME this is a dumb way to do this, clean it up later
if self.noise:
n_pos = np.random.normal(0,self.noise_std[0],3)
n_lin = np.random.normal(0,self.noise_std[1],3)
n_ori = np.random.normal(0,self.noise_std[2],3)
n_ang = np.random.normal(0,self.noise_std[3],3)
for i in range(3):
self.pos[i] += n_pos[i]
self.lin[i] += n_lin[i]
self.ori[i] += n_ori[i]
self.ang[i] += n_ang[i]
#TODO: might have to wrap angles here
# Find the error
self.ori_err = [self.t_ori[0] - self.ori[0],
self.t_ori[1] - self.ori[1],
self.t_ori[2] - self.ori[2]]
cz = math.cos(self.ori[2])
sz = math.sin(self.ori[2])
x_err = self.t_pos[0] - self.pos[0]
y_err = self.t_pos[1] - self.pos[1]
self.pos_err = [ x_err * cz + y_err * sz,
-x_err * sz + y_err * cz,
self.t_pos[2] - self.pos[2]]
self.lin = [self.lin[0]*cz+self.lin[1]*sz, -self.lin[0]*sz+self.lin[1]*cz, self.lin[2]]
self.ang = [self.ang[0]*cz+self.ang[1]*sz, -self.ang[0]*sz+self.ang[1]*cz, self.ang[2]]
for i in range(3):
if self.ori_err[i] > math.pi:
self.ori_err[i] -= 2 * math.pi
elif self.ori_err[i] < -math.pi:
self.ori_err[i] += 2 * math.pi
def run_random() :
import random
abs_angle = vrep.simxGetObjectOrientation(clientID, pioneerRobotHandle, -1, vrep.simx_opmode_oneshot)
random = random.random()
if random < 0.95 :
run_motor(MOTOR_FRONT, MID_SPEED, RUN_FRONT_TIME, 0)
elif random < 0.975 :
#RUN RIGHT
angle = convertAngle(abs_angle[1][2] + math.pi/4)
runPioneerToAngle(angle)
elif random < 1 :
#RUN LEFT
angle = convertAngle(abs_angle[1][2] - math.pi/4)
runPioneerToAngle(angle)
def getOrientation(self, name, relativeName=None):
"""
Get the orientation of an object
"""
if name in self.handles.keys():
pass
else:
self.initHandle(name)
robotHandle=self.handles[name]
if relativeName:
if relativeName in self.handles.keys():
pass
else:
self.initHandle(relativeName)
relativeHandle=self.handles[relativeName]
else:
relativeHandle = -1
returnCode,angles=vrep.simxGetObjectOrientation(self.clientID,robotHandle,relativeHandle,vrep.simx_opmode_streaming)
return angles
def observe(self):
operationMode=vrep.simx_opmode_streaming
if self.__initLoop:
self.__initLoop=False
else:
operationMode=vrep.simx_opmode_buffer
returnCode, orientation = vrep.simxGetObjectOrientation(self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode==vrep.simx_return_ok:
self.__orientation=orientation[2]
else:
self.__orientation = None
# print >> sys.stderr, "Error in VRepBubbleRob.getOrientation()"
self.__mind.setInput("orientation", self.__orientation)
returnCode, position = vrep.simxGetObjectPosition(self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode==vrep.simx_return_ok:
self.__position=[0.0,0.0]
self.__position[0]=position[0] #X
self.__position[1]=position[1] #Y
else:
self.__position=None
print >> sys.stderr, "Error in VRepBubbleRob.getPosition()", self.__clientID, self.__bodyHandle
returnCode, linearVelocity, angularVelocity = vrep.simxGetObjectVelocity(self.__clientID, self.__bodyHandle, operationMode)
if returnCode==vrep.simx_return_ok:
try:
# self.__velocity=linearVelocity[0]*math.cos(self.__orientation)+linearVelocity[1]*math.sin(self.__orientation)
self.__velocity=math.sqrt(linearVelocity[0]**2+linearVelocity[1]**2)
self.__mind.setInput("velocity", self.__velocity)
self.__linearVelocity=linearVelocity
except TypeError:
pass
# if self.__name=="BubbleRob#1":
# print self.__velocity, linearVelocity[0], math.cos(self.__orientation), linearVelocity[1], math.sin(self.__orientation)
else:
self.__velocity=None
# print >> sys.stderr, "Error in VRepBubbleRob.getVelocity()"
returnCode, sensorTrigger, dp, doh, dsnv = vrep.simxReadProximitySensor(self.__clientID, self.__sensorHandle, operationMode)
if returnCode==vrep.simx_return_ok:
# We succeeded at reading the proximity sensor
self.__mind.setInput("lastProximitySensorTime", vrep.simxGetLastCmdTime(self.__clientID))
self.__mind.setInput("sensorTrigger", sensorTrigger)
self.blocked() # judge if blocked
def loop(self):
operationMode=vrep.simx_opmode_streaming
if self.__initLoop:
self.__initLoop=False
else:
operationMode=vrep.simx_opmode_buffer
returnCode, orientation = vrep.simxGetObjectOrientation(self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode==vrep.simx_return_ok:
self.__orientation=orientation[2] #Z
else:
self.__orientation = None
print >> sys.stderr, "Error in VRepBubbleRob.getOrientation()"
returnCode, position = vrep.simxGetObjectPosition(self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode==vrep.simx_return_ok:
self.__position=[0.0,0.0]
self.__position[0]=position[0] #X
self.__position[1]=position[1] #Y
else:
print >> sys.stderr, "Error in VRepBubbleRob.getPosition()"
self.__position=None
returnCode, sensorTrigger, dp, doh, dsnv = vrep.simxReadProximitySensor(self.__clientID, self.__sensorHandle, operationMode)
if returnCode==vrep.simx_return_ok:
# We succeeded at reading the proximity sensor
simulationTime=vrep.simxGetLastCmdTime(self.__clientID)
thrust=0.0
steering=0.0
if simulationTime-self.__driveBackStartTime<3000:
# driving backwards while slightly turning:
thrust=-1.0
steering=-1.0
else:
# going forward:
thrust=1.0
steering=0.0
if sensorTrigger:
self.__driveBackStartTime=simulationTime # We detected something, and start the backward mode
self.setSteering(steering)
self.setThrust(thrust)
getSignalReturnCode, dMessage = vrep.simxGetStringSignal(self.__clientID, "Debug", vrep.simx_opmode_streaming)
if dMessage!="":
print("Debug:"+dMessage)
def initilize_vrep_api(self):
# initialize ePuck handles and variables
_, self.bodyelements=vrep.simxGetObjectHandle(
self.clientID, 'ePuck_bodyElements', vrep.simx_opmode_oneshot_wait)
_, self.leftMotor=vrep.simxGetObjectHandle(
self.clientID, 'ePuck_leftJoint', vrep.simx_opmode_oneshot_wait)
_, self.rightMotor=vrep.simxGetObjectHandle(
self.clientID, 'ePuck_rightJoint', vrep.simx_opmode_oneshot_wait)
_, self.ePuck=vrep.simxGetObjectHandle(
self.clientID, 'ePuck', vrep.simx_opmode_oneshot_wait)
_, self.ePuckBase=vrep.simxGetObjectHandle(
self.clientID, 'ePuck_base', vrep.simx_opmode_oneshot_wait)
# proxSens = prox_sens_initialize(self.clientID)
# initialize odom of ePuck
_, self.xyz = vrep.simxGetObjectPosition(
self.clientID, self.ePuck, -1, vrep.simx_opmode_streaming)
_, self.eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, self.ePuck, -1, vrep.simx_opmode_streaming)
# initialize overhead cam
_, self.overheadCam=vrep.simxGetObjectHandle(
self.clientID, 'Global_Vision', vrep.simx_opmode_oneshot_wait)
_, self.resolution, self.image = vrep.simxGetVisionSensorImage(
self.clientID,self.overheadCam,0,vrep.simx_opmode_oneshot_wait)
# initialize goal handle + odom
_, self.goalHandle=vrep.simxGetObjectHandle(
self.clientID, 'Goal_Position', vrep.simx_opmode_oneshot_wait)
_, self.goalPose = vrep.simxGetObjectPosition(
self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming)
# STOP Motor Velocities. Not sure if this is necessary
_ = vrep.simxSetJointTargetVelocity(
self.clientID,self.leftMotor,0,vrep.simx_opmode_oneshot_wait)
_ = vrep.simxSetJointTargetVelocity(
self.clientID,self.rightMotor,0,vrep.simx_opmode_oneshot_wait)
velizq= 0
velder= 0
oldPosition = [0,0,0]
oldOrientation = [0,0,0]
oldSpeed = [0,0,0]
oldAngularSpeed = [0,0,0]
angleModel = [0,0,0]
targetReached = False
speedMod = 1
(err, robot) = vrep.simxGetObjectHandle(clientID,"K3_robot",vrep.simx_opmode_oneshot_wait)
(err, rwmotor) = vrep.simxGetObjectHandle(clientID,"K3_rightWheelMotor#",vrep.simx_opmode_oneshot_wait)
(err, lwmotor) = vrep.simxGetObjectHandle(clientID,"K3_leftWheelMotor#",vrep.simx_opmode_oneshot_wait)
oldJoystick = data.find_one({"item": "joystick"})
while angleModel[1]==0:
(err, orientation) = vrep.simxGetObjectOrientation(clientID,robot,-1,vrep.simx_opmode_oneshot)
angleModel[1]= orientation[1]
angleModel[2] = abs(angleModel[1])
while clientID!=-1:
joystick = data.find_one({"_id": 0})
x = float(joystick['x'])
y = float(joystick['y'])
lockMode = str(joystick['mode'])
(err, position) = vrep.simxGetObjectPosition(clientID,robot,-1,vrep.simx_opmode_oneshot)
(err, orientation) = vrep.simxGetObjectOrientation(clientID,robot,-1,vrep.simx_opmode_oneshot)
(err, speed, angularSpeed) = vrep.simxGetObjectVelocity(clientID,robot,vrep.simx_opmode_oneshot)
angleModel[0] = orientation[1]
(velizq, velder, targetReached) = setSpeeds(x, y, angleModel, lockMode)
if (joystick != oldJoystick) or targetReached:
oldJoystick = joystick
print msg
logging.info(msg)
# Start getting the robot position
# Unlike other commands, we will use a streaming operating mode
# http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxGetObjectPosition
pret, robotPos = vrep.simxGetObjectPosition(clientID, robotHandle, -1, vrep.simx_opmode_streaming)
msg = "2w1a position: (x = " + str(robotPos[0]) +\
", y = " + str(robotPos[1]) + ") res = " + str(pret)
print msg
logging.info(msg)
# Start getting the robot orientation
# Unlike other commands, we will use a streaming operating mode
# http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxGetObjectOrientation
oret, robotOrient = vrep.simxGetObjectOrientation(clientID, robotHandle, -1, vrep.simx_opmode_streaming)
msg = "2w1a orientation: (x = " + str(robotOrient[0]) + \
", y = " + str(robotOrient[1]) +\
", z = " + str(robotOrient[2]) + ")"
print msg
logging.info(msg)
# define the population for evolution
population = []
# Make the robot move randomly
for j in range(0, 100):
vrep.simxStartSimulation(clientID, opmode)
# Generating random positions for the motors
awrist = random.randint(0, 300)
aelbow = random.randint(0, 300)
state = np.array([train])
state = state.astype(int)
proximitysensor = state[0][0], state[0][1], state[0][2]
loss_log = []
replay = []
data_collect = []
maxroute = 0
driveCount = 0
trainCount = 0
action = 2
new_action = 0
flipCount = 0
errorCode, collisionState1=vrep.simxReadCollision(clientID,collision_handle1,vrep.simx_opmode_streaming)
errorCode, eulerAngles=vrep.simxGetObjectOrientation(clientID,car_handle,-1,vrep.simx_opmode_streaming)
while trainCount < train_frames:
errorCode, eulerAngles=vrep.simxGetObjectOrientation(clientID,car_handle,-1,vrep.simx_opmode_buffer)
print (eulerAngles)
trainCount += 1
print(trainCount)
#Collision handler
errorCode, collisionState1=vrep.simxReadCollision(clientID,collision_handle1,vrep.simx_opmode_buffer)
print(collisionState1)
print(maxroute)
if abs(eulerAngles[0])>2:
returnCode=vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot)
def JointControl(clientID, motionProxy, postureProxy, i, Body):
# Head
# velocity_x = vrep.simxGetObjectFloatParameter(clientID,vrep.simxGetObjectHandle(clientID,'NAO#',vrep.simx_opmode_oneshot_wait)[1],11,vrep.simx_opmode_streaming)
pos = vrep.simxGetObjectPosition(clientID,vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_streaming)[1]
angularPos = vrep.simxGetObjectOrientation(clientID,vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_streaming)[1]
alphaPosRegister = []
betaPosRegister = []
gamaPosRegister = []
j = 0
li = []
while vrep.simxGetConnectionId(clientID) != -1:
for x in range(0, 150):
commandAngles = motionProxy.getAngles('Body', False)
j+=1
pos = vrep.simxGetObjectPosition(clientID, vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_buffer)[1]
angularPos = vrep.simxGetObjectOrientation(clientID, vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_buffer)[1]
alphaPosRegister.append(angularPos[0])
betaPosRegister.append(angularPos[1])
gamaPosRegister.append(angularPos[2])
vrep.simxSetJointTargetPosition(clientID,Body[0][i],commandAngles[0],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[1][i],commandAngles[1],vrep.simx_opmode_streaming)
# Left Leg
vrep.simxSetJointTargetPosition(clientID,Body[2][i],commandAngles[8],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[3][i],commandAngles[9],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[4][i],commandAngles[10],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[5][i],commandAngles[11],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[6][i],commandAngles[12],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[7][i],commandAngles[13],vrep.simx_opmode_streaming)
# Right Leg
vrep.simxSetJointTargetPosition(clientID,Body[8][i],commandAngles[14],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[9][i],commandAngles[15],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[10][i],commandAngles[16],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[11][i],commandAngles[17],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[12][i],commandAngles[18],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[13][i],commandAngles[19],vrep.simx_opmode_streaming)
# Left Arm
vrep.simxSetJointTargetPosition(clientID,Body[14][i],commandAngles[2],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[15][i],commandAngles[3],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[16][i],commandAngles[4],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[17][i],commandAngles[5],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[18][i],commandAngles[6],vrep.simx_opmode_streaming)
# Right Arm
vrep.simxSetJointTargetPosition(clientID,Body[19][i],commandAngles[20],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[20][i],commandAngles[21],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[21][i],commandAngles[22],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[22][i],commandAngles[23],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[23][i],commandAngles[24],vrep.simx_opmode_streaming)
# Left Fingers
vrep.simxSetJointTargetPosition(clientID,Body[25][i][0],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][1],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][2],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][3],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][4],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][5],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][6],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][7],1.0-commandAngles[7],vrep.simx_opmode_streaming)
# Right Fingers
vrep.simxSetJointTargetPosition(clientID,Body[27][i][0],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][1],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][2],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][3],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][4],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][5],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][6],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][7],1.0-commandAngles[25],vrep.simx_opmode_streaming)
motionProxy.stopMove()
postureProxy.stopMove()
# postureProxy.goToPosture("Stand",0.5)
vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
break
print 'End of simulation'
print 'Final x pos'
print pos
calculos = {}
calcular(alphaPosRegister,calculos)
desvpadA = calculos['desvio_padrao']
calculos = {}
calcular(betaPosRegister,calculos)
desvpadB = calculos['desvio_padrao']
calculos = {}
calcular(gamaPosRegister,calculos)
desvpadG = calculos['desvio_padrao']
return [(10*pos[0] - (desvpadA + desvpadB + desvpadG)/3), pos[0]]
ret_lm, leftMotorHandle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
ret_rm, rightMotorHandle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)
ret_pr, pioneerRobotHandle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
ret_pl, indoorPlantHandle = vrep.simxGetObjectHandle(clientID, 'indoorPlant', vrep.simx_opmode_oneshot_wait)
ret_sl, ultraSonicSensorStraightLeft = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor' + str(4),vrep.simx_opmode_oneshot_wait)
ret_sr, ultraSonicSensorStraightRight = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor' + str(5),vrep.simx_opmode_oneshot_wait)
ret_sr, ultraSonicSensorAngleLeft = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor' + str(3),vrep.simx_opmode_oneshot_wait)
ret_sr, ultraSonicSensorAngleRight = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor' + str(6),vrep.simx_opmode_oneshot_wait)
ret_sr, ultraSonicSensorLeft = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor' + str(1),vrep.simx_opmode_oneshot_wait)
ret_sr, ultraSonicSensorRight = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor' + str(8),vrep.simx_opmode_oneshot_wait)
ret_sr, ultraSonicSensorBackLeft = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor' + str(14),vrep.simx_opmode_oneshot_wait)
ret_sr, ultraSonicSensorBackRight = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor' + str(10),vrep.simx_opmode_oneshot_wait)
sensorHandleList=[(sensorNumber,vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor' + str(sensorNumber),vrep.simx_opmode_oneshot_wait)[1]) for sensorNumber in range(1,17) ]
error,currentOrientationEuler=vrep.simxGetObjectOrientation(clientID,pioneerRobotHandle,-1,vrep.simx_opmode_streaming)
while True: # main Control loop
#################################################
# Perception Phase: Get information about environment
##############################################
# distance to plant
pos2Plant = vrep.simxGetObjectPosition(clientID, pioneerRobotHandle, indoorPlantHandle, vrep.simx_opmode_oneshot_wait)
movePlant()
# distance to obstacle, return [distLeft, distRight]
dist2Obstacle= [(sensorNumber,getObstacleDist(sensor)) for sensorNumber,sensor in sensorHandleList]
########################################
# Reasoning: figure out which action to take
