def getObjectPositionWrapper(self, clientID, LSP_Handle):
if self.getObjectPositionFirstTime:
error, ret = vrep.simxGetObjectPosition(clientID, LSP_Handle, -1, vrep.simx_opmode_streaming)
self.getObjectPositionFirstTime = False
else:
error, ret = vrep.simxGetObjectPosition(clientID, LSP_Handle, -1, vrep.simx_opmode_buffer)
return error, ret
def handle_output(self):
# return whatever state information you need to get the error you want
# this will get you the information for the center of the object. If you
# want something like the position of the end of an arm, you will need
# to do some calculations, or just make a dummy object, attach it to
# the point you want, and get the position of the dummy object
#err, hand_pos = vrep.simxGetObjectPosition(self.cid, self.hand, -1,
# vrep.simx_opmode_oneshot)
#err, hand_ori = vrep.simxGetObjectOrientation(self.cid, self.hand, -1,
# vrep.simx_opmode_oneshot)
err, hand_ori = vrep.simxGetJointPosition(self.cid, self.hand_joint,
vrep.simx_opmode_oneshot)
err, arm_ori = vrep.simxGetJointPosition(self.cid, self.arm_joint,
vrep.simx_opmode_oneshot)
err, hand_vel = vrep.simxGetObjectFloatParameter(self.cid, self.hand_joint,
2012, vrep.simx_opmode_oneshot)
err, arm_vel = vrep.simxGetObjectFloatParameter(self.cid, self.arm_joint,
2012, vrep.simx_opmode_oneshot)
err, hand_pos = vrep.simxGetObjectPosition(self.cid, self.hand, -1,
vrep.simx_opmode_oneshot)
err, target_pos = vrep.simxGetObjectPosition(self.cid, self.target, -1,
vrep.simx_opmode_oneshot)
return [arm_ori, hand_ori, arm_vel, hand_vel, hand_pos[0], hand_pos[2],
target_pos[0], target_pos[1]]
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 simple():
# obtem os handlers. Um Handler eh um numero que identifica um componente, como, por exemplo, uma junta
res, nao = vrep.simxGetObjectHandle(clientID, "NAO", vrep.simx_opmode_blocking)
res, shL = vrep.simxGetObjectHandle(clientID, "LShoulderPitch3", vrep.simx_opmode_blocking)
res, shR = vrep.simxGetObjectHandle(clientID, "RShoulderPitch3", vrep.simx_opmode_blocking)
res, kneeR = vrep.simxGetObjectHandle(clientID, "RKneePitch3", vrep.simx_opmode_blocking)
res, kneeL = vrep.simxGetObjectHandle(clientID, "LKneePitch3", vrep.simx_opmode_blocking)
res, hipPitchL = vrep.simxGetObjectHandle(clientID, "LHipPitch3", vrep.simx_opmode_blocking)
res, hipPitchR = vrep.simxGetObjectHandle(clientID, "RHipPitch3", vrep.simx_opmode_blocking)
res, hipYawPitchL = vrep.simxGetObjectHandle(clientID, "LHipYawPitch3", vrep.simx_opmode_blocking)
res, hipYawPitchR = vrep.simxGetObjectHandle(clientID, "RHipYawPitch3", vrep.simx_opmode_blocking)
res, anklePitchL = vrep.simxGetObjectHandle(clientID, "LAnklePitch3", vrep.simx_opmode_blocking)
res, anklePitchR = vrep.simxGetObjectHandle(clientID, "RAnklePitch3", vrep.simx_opmode_blocking)
while True:
# Envia comandos para as juntas
vrep.simxSetJointTargetPosition(clientID, kneeR, 1.2,vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, anklePitchR, 0.4,vrep.simx_opmode_oneshot)
time.sleep(1)
vrep.simxSetJointTargetPosition(clientID, hipYawPitchL, -0.3,vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, hipPitchR, -0.1,vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, hipYawPitchR, -0.3,vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, kneeR, -0.1,vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, anklePitchR, 0,vrep.simx_opmode_oneshot)
print vrep.simxGetObjectPosition(clientID, nao, -1, vrep.simx_opmode_blocking)
reset_simulation(clientID)
def configure_handles(self):
# Handles of body and joints
rc, body = vrep.simxGetObjectHandle(self.client_id, 'NAO', vrep.simx_opmode_oneshot_wait)
assert rc == 0, rc
rc, joint_0 = vrep.simxGetObjectHandle(self.client_id, 'LShoulderPitch3', vrep.simx_opmode_oneshot_wait)
assert rc == 0, rc
rc, joint_1 = vrep.simxGetObjectHandle(self.client_id, 'RShoulderPitch3', vrep.simx_opmode_oneshot_wait)
assert rc == 0, rc
rc, joint_2 = vrep.simxGetObjectHandle(self.client_id, 'LHipPitch3', vrep.simx_opmode_oneshot_wait)
assert rc == 0, rc
rc, joint_3 = vrep.simxGetObjectHandle(self.client_id, 'RHipPitch3', vrep.simx_opmode_oneshot_wait)
assert rc == 0, rc
rc, joint_4 = vrep.simxGetObjectHandle(self.client_id, 'LKneePitch3', vrep.simx_opmode_oneshot_wait)
assert rc == 0, rc
rc, joint_5 = vrep.simxGetObjectHandle(self.client_id, 'RKneePitch3', vrep.simx_opmode_oneshot_wait)
assert rc == 0, rc
rc, joint_6 = vrep.simxGetObjectHandle(self.client_id, 'LAnklePitch3', vrep.simx_opmode_oneshot_wait)
assert rc == 0, rc
rc, joint_7 = vrep.simxGetObjectHandle(self.client_id, 'RAnklePitch3', vrep.simx_opmode_oneshot_wait)
assert rc == 0, rc
rc, joint_8 = vrep.simxGetObjectHandle(self.client_id, 'LHipYawPitch3', vrep.simx_opmode_oneshot_wait)
assert rc == 0, rc
rc, self.vase = vrep.simxGetObjectHandle(self.client_id, 'indoorPlant', vrep.simx_opmode_oneshot_wait)
assert rc == 0, rc
self.body = body
self.joints = [joint_0, joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7, joint_8]
self.get_body_position(initial=True)
self.get_joint_angles(initial=True)
vrep.simxGetObjectPosition(self.client_id, self.body, self.vase, vrep.simx_opmode_streaming)
time.sleep(0.5)
def getPosition(clientID,goal_new):
errorcode,newgoal_handle=vrep.simxGetObjectHandle(clientID,goal_new,vrep.simx_opmode_oneshot_wait)
#time.sleep(1)
errorCode,newgoal_position=vrep.simxGetObjectPosition(clientID,newgoal_handle,-1,vrep.simx_opmode_streaming)
time.sleep(1)
errorCode,newgoal_position=vrep.simxGetObjectPosition(clientID,newgoal_handle,-1,vrep.simx_opmode_buffer)
return newgoal_position
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 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 getState(self):
returnCode,position=vrep.simxGetObjectPosition(self.clientID,self.car,-1,vrep.simx_opmode_oneshot_wait)
returnCode,positionFR=vrep.simxGetObjectPosition(self.clientID,self.fr,-1,vrep.simx_opmode_oneshot_wait)
returnCode,positionBR=vrep.simxGetObjectPosition(self.clientID,self.br,-1,vrep.simx_opmode_oneshot_wait)
theta=math.atan2(positionFR[1]-positionBR[1],positionFR[0]-positionBR[0])
#return [position[0],position[1],theta,1*(self.redBoundaries[0]>position[0] or position[0]>self.redBoundaries[1]),self.robot.desiredWheelRotSpeed,self.robot.desiredSteeringAngle]
return [position[0],position[1],theta]
def getPosition(clientID, object_name):
# get the handle of the object, which position is needed
errorcode, object_handle = vrep.simxGetObjectHandle(clientID, object_name, vrep.simx_opmode_oneshot_wait)
# get the position
errorCode, object_position = vrep.simxGetObjectPosition(clientID, object_handle, -1, vrep.simx_opmode_streaming)
# some waiting time is needed to get the right data
time.sleep(1)
errorCode, object_position = vrep.simxGetObjectPosition(clientID, object_handle, -1, vrep.simx_opmode_buffer)
return object_position
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 load(self, com, base_h):
self.handle = com._vrep_get_handle(self.name)
res, pos = remote_api.simxGetObjectPosition(com.api_id, self.handle, base_h,
remote_api.simx_opmode_oneshot_wait)
assert res == 0
self.pos = tuple(100*p for p in pos)
res, pos_w = remote_api.simxGetObjectPosition(com.api_id, self.handle, -1,
remote_api.simx_opmode_oneshot_wait)
assert res == 0
self.pos_w = tuple(100*p for p in pos_w)
self.dim, self.mass = self.object_properties(com, self.handle)
def odometryData():
err_pos_o, pos_o = vrep.simxGetObjectPosition(Variables.clientID,
Variables.bodyHandle,
-1, vrep.simx_opmode_streaming)
err_pos_n, pos = vrep.simxGetObjectPosition(Variables.clientID,
Variables.bodyHandle,
-1, vrep.simx_opmode_buffer)
if Variables.robot_pos_new != pos:
Variables.robot_pos_old = Variables.robot_pos_new
Variables.robot_pos_new = pos
def getDistanceBetwObjects(self, objectNameA, objectNameB):
"""Get the distance between two named objects in V-REP.
Raise exception if either does not exist"""
eCode, handleA = vrep.simxGetObjectHandle(self._VREP_clientId, objectNameA, vrep.simx_opmode_oneshot_wait)
if eCode != 0:
raise Exception("Could not get handle of object", objectNameA)
eCode, poseA = vrep.simxGetObjectPosition(self._VREP_clientId, handleA, -1, vrep.simx_opmode_oneshot_wait)
eCode, handleB = vrep.simxGetObjectHandle(self._VREP_clientId, objectNameB, vrep.simx_opmode_oneshot_wait)
if eCode != 0:
raise Exception("Could not get handle of object", objectNameB)
eCode, poseB = vrep.simxGetObjectPosition(self._VREP_clientId, handleB, -1, vrep.simx_opmode_oneshot_wait)
return distance(poseA,poseB)
def __init__(self, clientID):
self.clientID = clientID
_, self.handle=vrep.simxGetObjectHandle(
self.clientID, 'Ball', vrep.simx_opmode_oneshot_wait)
vrep.simxGetObjectPosition(
self.clientID, self.handle, -1, vrep.simx_opmode_streaming)
self.posm2 = self.getBallPose()
self.tm2 = time.time()
self.posm1 = self.getBallPose()
self.tm1 = time.time()
self.pos = self.getBallPose()
self.t = time.time()
self.T = 2.15 # time constant in [s]
vrep.simxGetFloatSignal(self.clientID, 'simTime', vrep.simx_opmode_streaming )
def getTargetStart(cid,target):
start_pos = [0]
while (max(start_pos) == 0):
controller.controller_motor(cid, copter, target)
err, start_pos = vrep.simxGetObjectPosition(cid, target, -1, mode())
return err, np.asarray(start_pos)
def __init__(self, clientID):
self.clientID = clientID
err,leftMotorHandle=vrep.simxGetObjectHandle(clientID,"remoteApiControlledBubbleRobLeftMotor",vrep.simx_opmode_oneshot_wait)
if err==vrep.simx_error_noerror:
self.leftMotorHandle = leftMotorHandle
print('Get handle for left motor: {}'.format(leftMotorHandle))
else:
print('Error by getting handle for left motor: {}'.format(err))
err,rightMotorHandle=vrep.simxGetObjectHandle(clientID,"remoteApiControlledBubbleRobRightMotor",vrep.simx_opmode_oneshot_wait)
if err==vrep.simx_error_noerror:
self.rightMotorHandle = rightMotorHandle
print('Get handle for right motor: {}'.format(rightMotorHandle))
else:
print('Error by getting handle for right motor: {}'.format(err))
err,visionSensorHandle=vrep.simxGetObjectHandle(clientID,"Vision_sensor",vrep.simx_opmode_oneshot_wait)
if err==vrep.simx_error_noerror:
self.visionSensorHandle = visionSensorHandle
print('Get handle for vision sensor: {}'.format(visionSensorHandle))
else:
print('Error by getting handle for vision sensor: {}'.format(err))
err,bubbleRobHandle=vrep.simxGetObjectHandle(clientID,"remoteApiControlledBubbleRob",vrep.simx_opmode_oneshot_wait) #getHandle
if err==vrep.simx_error_noerror:
self.bubbleRobHandle = bubbleRobHandle
print('Get handle for bubbleRob: {}'.format(bubbleRobHandle))
else:
print('Error by getting handle for bubbleRob: {}'.format(err))
err,self.bubbleRobStartPosition = vrep.simxGetObjectPosition(clientID, bubbleRobHandle, -1, vrep.simx_opmode_oneshot_wait) #getStartPosition
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 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 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 loop(self):
operationMode = vrep.simx_opmode_streaming
if self.__initLoop:
self.__initLoop = False
else:
operationMode = vrep.simx_opmode_buffer
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.__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()"
self.__cnt = self.__cnt + 1
def run(path, handles):
files = []
for i in range(len(handles)):
files.append(open('path_%d.csv' % i, 'w'))
for state in path:
for i in range(len(handles)):
handle = handles[i]
ret, pos = vrep.simxGetObjectPosition(clientID, handle, -1, vrep.simx_opmode_oneshot_wait)
# wait until it's almost in the position
# ------
if (not ret == vrep.simx_return_ok):
print('Failed to retrieve position for Quadricopter_target')
sys.exit(1)
#print pos
new_pos = pos
new_pos[0] = state.positions[i].as_tuple()[0]
new_pos[1] = state.positions[i].as_tuple()[1]
files[i].write(state.positions[i].as_vrep_path_point())
ret = vrep.simxSetObjectPosition(clientID, handle, -1, new_pos, vrep.simx_opmode_oneshot_wait)
if (not ret == vrep.simx_return_ok):
print('Failed to set position for Quadricopter_target')
sys.exit(1)
for i in range(len(handles)):
files[i].close()
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 getNearestConcretBlockDist(clientID, blockHandleArray, pioneerRobotHandle):
distances = [0. for ix in range(len(blockHandleArray))] # initialize a container
pioneer2BlockPos = [[] for ix in range(len(blockHandleArray))] # initialize a container
for i_block in range(len(blockHandleArray)):
pioneer2BlockPos[i_block] = vrep.simxGetObjectPosition(clientID, pioneerRobotHandle, blockHandleArray[i_block][1], vrep.simx_opmode_oneshot_wait) # get relative position to robot
distances[i_block] = math.sqrt(pioneer2BlockPos[i_block][1][0]**2 + pioneer2BlockPos[i_block][1][1]**2) # compute Euclidean distance
#print(i_block, distances[i_block])
return min(distances), distances.index(min(distances)), pioneer2BlockPos[distances.index(min(distances))]
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 isRobotUp(clientID):
error, LSP_Handle=vrep.simxGetObjectHandle(clientID,"Bioloid", vrep.simx_opmode_oneshot_wait)
error, bioloid_position = vrep.simxGetObjectPosition(clientID, LSP_Handle, -1, vrep.simx_opmode_streaming)
#in case of problems consulting the robot position, consider that the robot is up
if error:
return error, True
else:
return error, bioloid_position[2]>FALL_THRESHOLD
def __init__(self, n_robots, base_name):
self.name = "line follower tester"
self.n_robots = n_robots
self.base_name = base_name
self.robot_names = [self.base_name]
for i in range(self.n_robots-1):
self.robot_names.append(self.base_name+'#'+str(i))
# Establish connection to V-REP
vrep.simxFinish(-1) # just in case, close all opened connections
# Connect to the simulation using V-REP's remote API (configured in V-REP, not scene specific)
# http://www.coppeliarobotics.com/helpFiles/en/remoteApiServerSide.htm
self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
# Use port 19999 and add simExtRemoteApiStart(19999) to some child-script in your scene for scene specific API
# (requires the simulation to be running)
if self.clientID != -1:
print ('Connected to remote API server')
else:
print ('Failed connecting to remote API server')
sys.exit('Could not connect')
# Reset running simulation
vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
time.sleep(0.2)
# Get initial robots' positions
self.robot_handles = []
self.robot_pos0 = []
for rbt_name in self.robot_names:
res, rbt_tmp = vrep.simxGetObjectHandle(self.clientID, rbt_name, vrep.simx_opmode_oneshot_wait)
self.robot_handles.append(rbt_tmp)
# Initialize data stream
vrep.simxGetObjectPosition(self.clientID, self.robot_handles[-1], -1, vrep.simx_opmode_streaming)
vrep.simxGetFloatSignal(self.clientID, rbt_name+'_1', vrep.simx_opmode_streaming)
vrep.simxGetFloatSignal(self.clientID, rbt_name+'_2', vrep.simx_opmode_streaming)
vrep.simxGetFloatSignal(self.clientID, rbt_name+'_3', vrep.simx_opmode_streaming)
time.sleep(0.2)
for rbt in self.robot_handles:
res, pos = vrep.simxGetObjectPosition(self.clientID, rbt, -1, vrep.simx_opmode_buffer)
self.robot_pos0.append(pos)
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 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 get_body_position(self, initial=False):
if initial:
mode = vrep.simx_opmode_streaming
else:
mode = vrep.simx_opmode_buffer
rc, body_position = vrep.simxGetObjectPosition(
self.client_id, self.body, -1, mode)
assert rc == (1 if initial else 0), rc
return np.array(body_position)
def QC_controller(Quadricopter_target, Quadricopter_base, Quadricopter,
Quadricopter_target2, Quadricopter_base2, Quadricopter2,
Quadricopter_target0, Quadricopter_base0, Quadricopter0,
Quadricopter_target1, Quadricopter_base1, Quadricopter1):
try:
import vrep
except:
print('--------------------------------------------------------------')
print('"vrep.py" could not be imported. This means very probably that')
print('either "vrep.py" or the remoteApi library could not be found.')
print('Make sure both are in the same folder as this file,')
print('or appropriately adjust the file "vrep.py"')
print('--------------------------------------------------------------')
print('')
""" =========================================================== """
""" ============ Imported Libraries: ============ """
import time
import numpy as np
from captains_log_v1 import log_data
import os
import cost
import csv
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import main_PID_controller
""" =================================================================== """
# output limits:
#min_output=0
#max_output=8.335
# program parameters:
global i
i = 0
xl = []
yl = []
yf = []
zl = []
xf2 = []
yf2 = []
zf2 = []
xf0 = []
yf0 = []
zf0 = []
xf1 = []
yf1 = []
zf1 = []
# xe = []
# ye = []
ze = []
# xs = []
# ys = []
# zs = []
x_qc = []
y_qc = []
z_qc = []
x_qc2 = []
y_qc2 = []
z_qc2 = []
# u = []
v1 = []
v2 = []
v3 = []
v4 = []
w1 = []
w2 = []
w3 = []
w4 = []
v12 = []
v22 = []
v32 = []
v42 = []
#global variables:
cumul = 0
last_e = 0
pAlphaE = 0
pBetaE = 0
psp2 = 0
psp1 = 0
prevEuler = 0
cumulAlpha = 0
cumulBeta = 0
cumulAlphaPos = 0
cumulBetaPos = 0
cumul2 = 0
last_e2 = 0
pAlphaE2 = 0
pBetaE2 = 0
psp22 = 0
psp12 = 0
prevEuler2 = 0
cumulAlpha2 = 0
cumulBeta2 = 0
cumulAlphaPos2 = 0
cumulBetaPos2 = 0
cumul0 = 0
last_e0 = 0
pAlphaE0 = 0
pBetaE0 = 0
psp20 = 0
psp10 = 0
prevEuler0 = 0
cumulAlpha0 = 0
cumulBeta0 = 0
cumulAlphaPos0 = 0
cumulBetaPos0 = 0
cumul1 = 0
last_e1 = 0
pAlphaE1 = 0
pBetaE1 = 0
psp21 = 0
psp11 = 0
prevEuler1 = 0
cumulAlpha1 = 0
cumulBeta1 = 0
cumulAlphaPos1 = 0
cumulBetaPos1 = 0
cumulPOS = 0
cumulVY = 0
cumulVX = 0
cumulSP0 = 0
cumulSP1 = 0
cumulEULER = 0
cumulPOS2 = 0
cumulVY2 = 0
cumulVX2 = 0
cumulSP02 = 0
cumulSP12 = 0
cumulEULER2 = 0
cumulPOS0 = 0
cumulVY0 = 0
cumulVX0 = 0
cumulSP00 = 0
cumulSP10 = 0
cumulEULER0 = 0
cumulPOS1 = 0
cumulVY1 = 0
cumulVX1 = 0
cumulSP01 = 0
cumulSP11 = 0
cumulEULER1 = 0
particlesTargetVelocities = [0, 0, 0, 0]
particlesTargetVelocities2 = [0, 0, 0, 0]
particlesTargetVelocities0 = [0, 0, 0, 0]
particlesTargetVelocities1 = [0, 0, 0, 0]
#speed weight:
vParam = -2
#parameters for vertical control
Kpv = 2
Kiv = 0
Kdv = 2
#parameters for horizontal control:
Kph = 0.4 # TBD
Kih = 0.1 # TBD
Kdh = 1.5
Kph_pos1 = 0.4
Kih_pos1 = 0.001
Kdh_pos1 = 0.05
Kph_pos0 = 0.4
Kih_pos0 = 0.001
Kdh_pos0 = 0.05
#parameters for rotational control:
Kpr = 0.05
# Kir=0
Kdr = 0.9
""" =========================================================== """
gd = 0
""" =========================================================== """
print('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000,
5) # Connect to V-REP
if clientID != -1:
print('Connected to remote API server')
# enable the synchronous mode on the client:
vrep.simxSynchronous(clientID, True)
# start the simulation:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
#functional/handle code:
emptyBuff = bytearray()
[returnCode,
targetObj] = vrep.simxGetObjectHandle(clientID, Quadricopter_target,
vrep.simx_opmode_blocking)
[returnCode,
qc_base_handle] = vrep.simxGetObjectHandle(clientID,
Quadricopter_base,
vrep.simx_opmode_blocking)
[returnCode,
qc_handle] = vrep.simxGetObjectHandle(clientID, Quadricopter,
vrep.simx_opmode_blocking)
[returnCode,
targetObj2] = vrep.simxGetObjectHandle(clientID, Quadricopter_target2,
vrep.simx_opmode_blocking)
[returnCode,
qc_base_handle2] = vrep.simxGetObjectHandle(clientID,
Quadricopter_base2,
vrep.simx_opmode_blocking)
[returnCode,
qc_handle2] = vrep.simxGetObjectHandle(clientID, Quadricopter2,
vrep.simx_opmode_blocking)
[returnCode,
targetObj0] = vrep.simxGetObjectHandle(clientID, Quadricopter_target0,
vrep.simx_opmode_blocking)
[returnCode,
qc_base_handle0] = vrep.simxGetObjectHandle(clientID,
Quadricopter_base0,
vrep.simx_opmode_blocking)
[returnCode,
qc_handle0] = vrep.simxGetObjectHandle(clientID, Quadricopter0,
vrep.simx_opmode_blocking)
[returnCode,
targetObj1] = vrep.simxGetObjectHandle(clientID, Quadricopter_target1,
vrep.simx_opmode_blocking)
[returnCode,
qc_base_handle1] = vrep.simxGetObjectHandle(clientID,
Quadricopter_base1,
vrep.simx_opmode_blocking)
[returnCode,
qc_handle1] = vrep.simxGetObjectHandle(clientID, Quadricopter1,
vrep.simx_opmode_blocking)
# main loop:
while True:
""" ========== vertical control: ========== """
[returnCode,
targetPos] = vrep.simxGetObjectPosition(clientID, targetObj, -1,
vrep.simx_opmode_blocking)
[returnCode,
pos] = vrep.simxGetObjectPosition(clientID, qc_base_handle, -1,
vrep.simx_opmode_blocking)
[returnCode, l,
w] = vrep.simxGetObjectVelocity(clientID, qc_base_handle,
vrep.simx_opmode_blocking)
[returnCode, targetPos2
] = vrep.simxGetObjectPosition(clientID, targetObj2, -1,
vrep.simx_opmode_blocking)
[returnCode,
pos2] = vrep.simxGetObjectPosition(clientID, qc_base_handle2, -1,
vrep.simx_opmode_blocking)
[returnCode, l2,
w2] = vrep.simxGetObjectVelocity(clientID, qc_base_handle2,
vrep.simx_opmode_blocking)
[returnCode, targetPos0
] = vrep.simxGetObjectPosition(clientID, targetObj0, -1,
vrep.simx_opmode_blocking)
[returnCode,
pos0] = vrep.simxGetObjectPosition(clientID, qc_base_handle0, -1,
vrep.simx_opmode_blocking)
[returnCode, l0,
w0] = vrep.simxGetObjectVelocity(clientID, qc_base_handle0,
vrep.simx_opmode_blocking)
[returnCode, targetPos1
] = vrep.simxGetObjectPosition(clientID, targetObj1, -1,
vrep.simx_opmode_blocking)
[returnCode,
pos1] = vrep.simxGetObjectPosition(clientID, qc_base_handle1, -1,
vrep.simx_opmode_blocking)
[returnCode, l1,
w1] = vrep.simxGetObjectVelocity(clientID, qc_base_handle1,
vrep.simx_opmode_blocking)
""" =========================================================== """
""" ========== horizontal control: ========== """
[returnCode,
sp] = vrep.simxGetObjectPosition(clientID, targetObj,
qc_base_handle,
vrep.simx_opmode_blocking)
[rc, rc, vx, rc, rc] = vrep.simxCallScriptFunction(
clientID, Quadricopter, vrep.sim_scripttype_childscript,
'qc_Get_vx', [], [], [], emptyBuff, vrep.simx_opmode_blocking)
[rc, rc, vy, rc, rc] = vrep.simxCallScriptFunction(
clientID, Quadricopter, vrep.sim_scripttype_childscript,
'qc_Get_vy', [], [], [], emptyBuff, vrep.simx_opmode_blocking)
[rc, rc, rc, rc,
rc] = vrep.simxCallScriptFunction(clientID, Quadricopter,
vrep.sim_scripttype_childscript,
'qc_Get_Object_Matrix', [], [],
[], emptyBuff,
vrep.simx_opmode_blocking)
[errorCode,
M] = vrep.simxGetStringSignal(clientID, 'mtable',
vrep.simx_opmode_oneshot_wait)
if (errorCode == vrep.simx_return_ok):
m = vrep.simxUnpackFloats(M)
m1 = m[0]
m2 = m[1]
m0 = m[2]
m11 = m[3]
vx1 = [vx[0], vx[1], vx[2]]
vx2 = [vx[3], vx[4], vx[5]]
vx0 = [vx[6], vx[7], vx[8]]
vx11 = [vx[9], vx[10], vx[11]]
vy1 = [vy[0], vy[1], vy[2]]
vy2 = [vy[3], vy[4], vy[5]]
vy0 = [vy[6], vy[7], vy[8]]
vy11 = [vy[9], vy[10], vy[11]]
[returnCode,
sp2] = vrep.simxGetObjectPosition(clientID, targetObj2,
qc_base_handle2,
vrep.simx_opmode_blocking)
[returnCode,
sp0] = vrep.simxGetObjectPosition(clientID, targetObj0,
qc_base_handle0,
vrep.simx_opmode_blocking)
[returnCode,
sp1] = vrep.simxGetObjectPosition(clientID, targetObj1,
qc_base_handle1,
vrep.simx_opmode_blocking)
""" =========================================================== """
""" ========== rotational control: ========== """
[returnCode,
euler] = vrep.simxGetObjectOrientation(clientID, targetObj,
qc_base_handle,
vrep.simx_opmode_blocking)
[returnCode, orientation
] = vrep.simxGetObjectOrientation(clientID, qc_base_handle, -1,
vrep.simx_opmode_blocking)
[returnCode,
euler2] = vrep.simxGetObjectOrientation(clientID, targetObj2,
qc_base_handle2,
vrep.simx_opmode_blocking)
[returnCode, orientation2
] = vrep.simxGetObjectOrientation(clientID, qc_base_handle2, -1,
vrep.simx_opmode_blocking)
[returnCode,
euler0] = vrep.simxGetObjectOrientation(clientID, targetObj0,
qc_base_handle0,
vrep.simx_opmode_blocking)
[returnCode, orientation0
] = vrep.simxGetObjectOrientation(clientID, qc_base_handle0, -1,
vrep.simx_opmode_blocking)
[returnCode,
euler1] = vrep.simxGetObjectOrientation(clientID, targetObj1,
qc_base_handle1,
vrep.simx_opmode_blocking)
[returnCode, orientation1
] = vrep.simxGetObjectOrientation(clientID, qc_base_handle1, -1,
vrep.simx_opmode_blocking)
""" =========================================================== """
""" ========== set propeller velocities: ========== """
particlesTargetVelocities, cumul, last_e, cumulAlpha, pAlphaE, cumulBeta, pBetaE, cumulAlphaPos, cumulBetaPos, psp2, psp1, prevEuler, cumulPOS, cumulVY, cumulVX, cumulSP0, cumulSP1, cumulEULER = main_PID_controller.PID(
targetPos, pos, cumul, last_e, Kpv, Kiv, Kdv, vParam, l, vy1,
m1, cumulAlpha, pAlphaE, Kph, Kih, Kdh, vx1, cumulBeta, pBetaE,
cumulAlphaPos, cumulBetaPos, sp, Kph_pos1, Kih_pos1, Kdh_pos1,
Kph_pos0, Kih_pos0, Kdh_pos0, psp2, psp1, Kpr, Kdr, euler,
prevEuler, cumulPOS, cumulVY, cumulVX, cumulSP0, cumulSP1,
cumulEULER, vx, vy, pos, sp, euler)
particlesTargetVelocities2, cumul2, last_e2, cumulAlpha2, pAlphaE2, cumulBeta2, pBetaE2, cumulAlphaPos2, cumulBetaPos2, psp22, psp12, prevEuler2, cumulPOS2, cumulVY2, cumulVX2, cumulSP02, cumulSP12, cumulEULER2 = main_PID_controller.PID(
targetPos2, pos2, cumul2, last_e2, Kpv, Kiv, Kdv, vParam, l2,
vy2, m2, cumulAlpha2, pAlphaE2, Kph, Kih, Kdh, vx2, cumulBeta2,
pBetaE2, cumulAlphaPos2, cumulBetaPos2, sp2, Kph_pos1,
Kih_pos1, Kdh_pos1, Kph_pos0, Kih_pos0, Kdh_pos0, psp22, psp12,
Kpr, Kdr, euler2, prevEuler2, cumulPOS2, cumulVY2, cumulVX2,
cumulSP02, cumulSP12, cumulEULER2, vx, vy, pos, sp, euler)
particlesTargetVelocities0, cumul0, last_e0, cumulAlpha0, pAlphaE0, cumulBeta0, pBetaE0, cumulAlphaPos0, cumulBetaPos0, psp20, psp10, prevEuler0, cumulPOS0, cumulVY0, cumulVX0, cumulSP00, cumulSP10, cumulEULER0 = main_PID_controller.PID(
targetPos0, pos0, cumul0, last_e0, Kpv, Kiv, Kdv, vParam, l0,
vy0, m0, cumulAlpha0, pAlphaE0, Kph, Kih, Kdh, vx0, cumulBeta0,
pBetaE0, cumulAlphaPos0, cumulBetaPos0, sp0, Kph_pos1,
Kih_pos1, Kdh_pos1, Kph_pos0, Kih_pos0, Kdh_pos0, psp20, psp10,
Kpr, Kdr, euler0, prevEuler0, cumulPOS0, cumulVY0, cumulVX0,
cumulSP00, cumulSP10, cumulEULER0, vx, vy, pos, sp, euler)
particlesTargetVelocities1, cumul1, last_e1, cumulAlpha1, pAlphaE1, cumulBeta1, pBetaE1, cumulAlphaPos1, cumulBetaPos1, psp21, psp11, prevEuler1, cumulPOS1, cumulVY1, cumulVX1, cumulSP01, cumulSP11, cumulEULER1 = main_PID_controller.PID(
targetPos1, pos1, cumul1, last_e1, Kpv, Kiv, Kdv, vParam, l1,
vy11, m11, cumulAlpha1, pAlphaE1, Kph, Kih, Kdh, vx11,
cumulBeta1, pBetaE1, cumulAlphaPos1, cumulBetaPos1, sp1,
Kph_pos1, Kih_pos1, Kdh_pos1, Kph_pos0, Kih_pos0, Kdh_pos0,
psp21, psp11, Kpr, Kdr, euler1, prevEuler1, cumulPOS1,
cumulVY1, cumulVX1, cumulSP01, cumulSP11, cumulEULER1, vx, vy,
pos, sp, euler)
particlesTargetVelocities_s = [
particlesTargetVelocities[0], particlesTargetVelocities[1],
particlesTargetVelocities[2], particlesTargetVelocities[3],
particlesTargetVelocities2[0], particlesTargetVelocities2[1],
particlesTargetVelocities2[2], particlesTargetVelocities2[3],
particlesTargetVelocities0[0], particlesTargetVelocities0[1],
particlesTargetVelocities0[2], particlesTargetVelocities0[3],
particlesTargetVelocities1[0], particlesTargetVelocities1[1],
particlesTargetVelocities1[2], particlesTargetVelocities1[3]
]
""" =========================================================== """
""" =========================================================== """
""" ========== PLOTTING: ========== """
## PLOTTING:
# ze.append(targetPos[2]-pos[2]) # otstapuvanje od z-oska
# xe.append(sp[0]) # otstapuvanje od x-oska
# ye.append(sp[1]) # otstapuvawe od y-oska
# xs.append(targetPos[0])
# ys.append(targetPos[1])
# zs.append(targetPos[2])
# x_qc.append(pos[0])
# y_qc.append(pos[1])
# z_qc.append(pos[2])
# x_qc2.append(pos2[0])
# y_qc2.append(pos2[1])
# z_qc2.append(pos2[2])
# fig = plt.figure()
# ax = fig.add_subplot(111, projection='3d')
# ax.plot_wireframe(x_qc,y_qc,z_qc,color='blue')
# ax.plot_wireframe(x_qc2,y_qc2,z_qc2,color='red')
# ax.set_xlim3d(-2.1,2.1)
# ax.set_ylim3d(-2.1,2.1)
# ax.set_zlim3d(0,1.9)
# plt.show()
# u.append(thrust)
''' !!! treba da se nacrta i pozicijata i da se sporedi so i bez consensus !!! '''
xl.append(pos[0])
yl.append(pos[1])
zl.append(pos[2])
xf2.append(pos2[0])
yf2.append(pos2[1])
zf2.append(pos2[2])
xf0.append(pos0[0])
yf0.append(pos0[1])
zf0.append(pos0[2])
xf1.append(pos1[0])
yf1.append(pos1[1])
zf1.append(pos1[2])
plt.plot(zl, color="blue")
plt.hold(True)
plt.plot(zf2, color="red")
plt.hold(True)
plt.plot(zf0, color="pink")
plt.hold(True)
plt.plot(zf1, color="green")
plt.hold(True)
plt.axis([0, 199, 0, 1.4])
plt.show()
plt.plot(xl, color="blue")
plt.hold(True)
plt.plot(xf2, color="red")
plt.hold(True)
plt.plot(xf0, color="pink")
plt.hold(True)
plt.plot(xf1, color="green")
plt.hold(True)
plt.axis([0, 199, -2, 2])
plt.show()
plt.plot(yl, color="blue")
plt.hold(True)
plt.plot(yf2, color="red")
plt.hold(True)
plt.plot(yf0, color="pink")
plt.hold(True)
plt.plot(yf1, color="green")
plt.hold(True)
plt.axis([0, 199, -2, 1.5])
plt.show()
v1.append(l[2])
v2.append(l2[2])
v3.append(l0[2])
v4.append(l1[2])
plt.plot(v1, color="blue")
plt.hold(True)
plt.plot(v2, color="red")
plt.hold(True)
plt.plot(v3, color="pink")
plt.hold(True)
plt.plot(v4, color="green")
plt.hold(True)
plt.axis([0, 199, -0.1, 1])
plt.show()
w1.append(w[2])
w2.append(w2[2])
w3.append(w0[2])
w4.append(w1[2])
plt.plot(w1, color="blue")
plt.hold(True)
plt.plot(w2, color="red")
plt.hold(True)
plt.plot(w3, color="pink")
plt.hold(True)
plt.plot(w4, color="green")
plt.hold(True)
plt.axis([0, 199, -0.5, 0.5])
plt.show()
# v12.append(particlesTargetVelocities2[0])
# v22.append(particlesTargetVelocities2[1])
# v32.append(particlesTargetVelocities2[2])
# v42.append(particlesTargetVelocities2[3])
# plt.plot(yl,color='blue')
# plt.hold(True)
# plt.plot(yf,color='red')
# plt.plot(euler_l,color='blue')
# plt.plot(euler_f,color='red')
# plt.plot(yl,color='green')
# plt.hold(True)
# plt.plot(yf,color='pink')
# plt.hold(True)
# plt.plot(v3,color='green')
# plt.hold(True)
# plt.plot(v4,color='pink')
# plt.hold(True)
# plt.axis([0,149,-3,3])
# plt.show()
# print " Zaxis error between drones: ", pos[2]-pos2[2]
ze.append(pos[2] - pos2[2])
# ze.append(abs(euler[2])-abs(euler2[2]))
# plt.plot(ze,color='green')
# plt.axis([0,149,-3,3])
# plt.show()
""" =========================================================== """
""" ========== Gradient descent: ========== """
# sum_h_alpha.append(cumulAlpha)
# sum_h_beta.append(cumulBeta)
# sum_h_pos1.append(cumulAlphaPos)
# sum_h_pos0.append(cumulBetaPos)
# J_h_alpha.append((1/(tf - t0))*(5*cumulAlpha**2 + delta_alpha_angle[gd]**2)*dt)
# J_h_beta.append((1/(tf - t0))*(5*cumulBeta**2 + delta_beta_angle[gd]**2)*dt)
# J_h_pos1.append((1/(tf - t0))*(5*cumulAlphaPos**2 + delta_alpha_pos[gd]**2)*dt)
# J_h_pos0.append((1/(tf - t0))*(5*cumulBetaPos**2 + delta_beta_pos[gd]**2)*dt)
# paramX.append(targetPos[0])
# paramY.append(targetPos[1])
# print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
# print "theta_h_angles : ", theta_h_angles
# print "theta_h_pos : ", theta_h_pos
#
# print "sum_h_alpha : ", sum_h_alpha[gd]
# print "sum_h_beta : ", sum_h_beta[gd]
# print "sum_h_pos1 : ", sum_h_pos1[gd]
# print "sum_h_pos0 : ", sum_h_pos0[gd]
# print "J_h_alpha : ", J_h_alpha[gd]
# print "J_h_beta : ", J_h_beta[gd]
# print "J_h_pos1 : ", J_h_pos1[gd]
# print "J_h_pos0 : ", J_h_pos0[gd]
# print "paramX : ", paramX[gd]
# print "paramY : ", paramY[gd]
# print "cumulPOS : ", cumulPOS2
# print "cumulVY : ", cumulVY2
# print "cumulVX : ", cumulVX2
# print "cumulSP0 : ", cumulSP02
# print "cumulSP1 : ", cumulSP12
# print "cumulEULER : ", cumulEULER2
# print "cumul : ", cumul
print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
gd = gd + 1
print "gd (iterations) : ", gd
print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
""" =========================================================== """
if gd == 200:
# with open('GD_43.csv','wb') as f1:
# writer=csv.writer(f1)
# for i in range(0, gd):
# writer.writerow([paramX[i],paramY[i],theta_h_angles[0],theta_h_angles[1],theta_h_angles[2],theta_h_pos[0],theta_h_pos[1],theta_h_pos[2],J_h_alpha[i],J_h_beta[i],J_h_pos1[i],J_h_pos0[i]])
break
# """ =========================================================== """
""" ========== WRITE TO TEXT: ========== """
## WRITE TO TEXT:
# log_data(C_parameters_vert, C_parameters_horr, C_parameters_rot, vert_comp, horr_comp, rot_comp, particlesTargetVelocities, i)
# i +=1
""" =========================================================== """
# send propeller velocities to output:
[res, retInts, retFloats, retStrings,
retBuffer] = vrep.simxCallScriptFunction(
clientID, Quadricopter, vrep.sim_scripttype_childscript,
'qc_propeller_v', [], particlesTargetVelocities_s, [],
emptyBuff, vrep.simx_opmode_blocking)
# [res,retInts,retFloats,retStrings,retBuffer]=vrep.simxCallScriptFunction(clientID,Quadricopter2,vrep.sim_scripttype_childscript,'qc_propeller_v',[],particlesTargetVelocities2,[],emptyBuff,vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(clientID)
""" =========================================================== """
# print " Zaxis error between drones: ", ze
# stop the simulation:
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
def initialize_handles(self):
self._RightMotor = self._vrep_get_object_handle(
'Right_Motor{}'.format(self._value_number),
vrep.simx_opmode_blocking)
self._LeftMotor = self._vrep_get_object_handle(
'Left_Motor{}'.format(self._value_number),
vrep.simx_opmode_blocking)
self._Robobo = self._vrep_get_object_handle(
'Robobo{}'.format(self._value_number), vrep.simx_opmode_blocking)
self._IrBackC = self._vrep_get_object_handle(
'Ir_Back_C{}'.format(self._value_number),
vrep.simx_opmode_blocking)
self._IrFrontC = self._vrep_get_object_handle(
'Ir_Front_C{}'.format(self._value_number),
vrep.simx_opmode_blocking)
self._IrFrontLL = self._vrep_get_object_handle(
'Ir_Front_LL{}'.format(self._value_number),
vrep.simx_opmode_blocking)
self._IrFrontRR = self._vrep_get_object_handle(
'Ir_Front_RR{}'.format(self._value_number),
vrep.simx_opmode_blocking)
self._IrBackL = self._vrep_get_object_handle(
'Ir_Back_L{}'.format(self._value_number),
vrep.simx_opmode_blocking)
self._IrBackLFloor = self._vrep_get_object_handle(
'Ir_Back_L_Floor{}'.format(self._value_number),
vrep.simx_opmode_blocking)
self._IrBackR = self._vrep_get_object_handle(
'Ir_Back_R{}'.format(self._value_number),
vrep.simx_opmode_blocking)
self._IrBackRFloor = self._vrep_get_object_handle(
'Ir_Back_R_Floor{}'.format(self._value_number),
vrep.simx_opmode_blocking)
self._IrFrontL = self._vrep_get_object_handle(
'Ir_Front_L{}'.format(self._value_number),
vrep.simx_opmode_blocking)
self._IrFrontLFloor = self._vrep_get_object_handle(
'Ir_Front_L_Floor{}'.format(self._value_number),
vrep.simx_opmode_blocking)
self._IrFrontR = self._vrep_get_object_handle(
'Ir_Front_R{}'.format(self._value_number),
vrep.simx_opmode_blocking)
self._IrFrontRFloor = self._vrep_get_object_handle(
'Ir_Front_R_Floor{}'.format(self._value_number),
vrep.simx_opmode_blocking)
self._TiltMotor = self._vrep_get_object_handle(
'Tilt_Motor{}'.format(self._value_number),
vrep.simx_opmode_blocking)
self._PanMotor = self._vrep_get_object_handle(
'Pan_Motor{}'.format(self._value_number),
vrep.simx_opmode_blocking)
self._FrontalCamera = self._vrep_get_object_handle(
'Frontal_Camera{}'.format(self._value_number),
vrep.simx_opmode_blocking)
# read a first value in streaming mode
self._vrep_read_proximity_sensor_ignore_error(self._IrFrontC)
self._vrep_read_proximity_sensor_ignore_error(self._IrBackC)
self._vrep_read_proximity_sensor_ignore_error(self._IrFrontLL)
self._vrep_read_proximity_sensor_ignore_error(self._IrFrontRR)
self._vrep_read_proximity_sensor_ignore_error(self._IrBackL)
self._vrep_read_proximity_sensor_ignore_error(self._IrBackLFloor)
self._vrep_read_proximity_sensor_ignore_error(self._IrBackR)
self._vrep_read_proximity_sensor_ignore_error(self._IrBackRFloor)
self._vrep_read_proximity_sensor_ignore_error(self._IrFrontR)
self._vrep_read_proximity_sensor_ignore_error(self._IrFrontRFloor)
self._vrep_read_proximity_sensor_ignore_error(self._IrFrontL)
self._vrep_read_proximity_sensor_ignore_error(self._IrFrontLFloor)
# setup join positions
vrep.simxGetJointPosition(self._clientID, self._RightMotor,
vrep.simx_opmode_buffer)
vrep.simxGetJointPosition(self._clientID, self._LeftMotor,
vrep.simx_opmode_buffer)
vrep.simxGetObjectPosition(self._clientID, self._Robobo, -1,
vrep.simx_opmode_buffer)
# read a first value in buffer mode
self._vrep_get_vision_sensor_image_ignore_error(
self._FrontalCamera, vrep.simx_opmode_streaming)
self.wait_for_ping()
def GetSuctionAbsPosition():
result, position = vrep.simxGetObjectPosition(clientID, suction, -1,
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get suction position')
return position
def run_sim(model, clientID): vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait) # start simulation previousPosition = [-2.71879,-1.90075,36.263,1.35473,0.780061,-35.7055,-0.225083,-3.02226,1.82057,0.258116,-2.12325,1.061] #RhipX,RhipY,RhipZ,LhipX,LhipY,LhipZ,RKneeZ,LKneeZ,RAnkleX,RAnkleZ,LAnkleX,LAnkleZ legJoints = [0,0,0,0,0,0,0,0,0,0,0,0] legJointInv = [1,1,1,-1,-1,1,1,1,1,1,-1,1] hipJointInv = [1,1,1,1,1,-1,1,1,1,1,1,1] returnCode01, legJoints[0] = vrep.simxGetObjectHandle(clientID, "rightLegJoint0", vrep.simx_opmode_blocking) returnCode02, legJoints[1] = vrep.simxGetObjectHandle(clientID, "rightLegJoint1", vrep.simx_opmode_blocking) returnCode03, legJoints[2] = vrep.simxGetObjectHandle(clientID, "rightLegJoint2", vrep.simx_opmode_blocking) returnCode04, legJoints[6] = vrep.simxGetObjectHandle(clientID, "rightLegJoint3", vrep.simx_opmode_blocking) returnCode05, legJoints[8] = vrep.simxGetObjectHandle(clientID, "rightLegJoint5", vrep.simx_opmode_blocking) returnCode06, legJoints[9] = vrep.simxGetObjectHandle(clientID, "rightLegJoint4", vrep.simx_opmode_blocking) returnCode07, legJoints[3] = vrep.simxGetObjectHandle(clientID, "leftLegJoint0", vrep.simx_opmode_blocking) returnCode08, legJoints[4] = vrep.simxGetObjectHandle(clientID, "leftLegJoint1", vrep.simx_opmode_blocking) returnCode09, legJoints[5] = vrep.simxGetObjectHandle(clientID, "leftLegJoint2", vrep.simx_opmode_blocking) returnCode010, legJoints[7] = vrep.simxGetObjectHandle(clientID, "leftLegJoint3", vrep.simx_opmode_blocking) returnCode011, legJoints[10] = vrep.simxGetObjectHandle(clientID, "leftLegJoint5", vrep.simx_opmode_blocking) returnCode012, legJoints[11] = vrep.simxGetObjectHandle(clientID, "leftLegJoint4", vrep.simx_opmode_blocking) returnCode013, head = vrep.simxGetObjectHandle(clientID, "Asti", vrep.simx_opmode_blocking) returnCode013, r_foot = vrep.simxGetObjectHandle(clientID, "rightFoot", vrep.simx_opmode_blocking) returnCode013, l_foot = vrep.simxGetObjectHandle(clientID, "leftFoot", vrep.simx_opmode_blocking) #RhipX,RhipY,RhipZ,LhipX,LhipY,LhipZ,RKneeZ,LKneeZ,RAnkleX,RAnkleZ,LAnkleX,LAnkleZ legPositions = [0,0,0,0,0,0,0,0,0,0,0,0] headLocation = [1,1,1]; begin_r = [0,0,0] begin_l = [0,0,0] end_r = [0,0,0] end_r = [0,0,0] for i in range(12): returnCode, legPositions[i] = vrep.simxGetJointPosition(clientID, legJoints[i], vrep.simx_opmode_streaming) returncode, headLocation = vrep.simxGetObjectPosition(clientID, head, -1, vrep.simx_opmode_streaming) returnCode, begin_r = vrep.simxGetObjectPosition(clientID, r_foot, -1, vrep.simx_opmode_streaming) returnCode, begin_l = vrep.simxGetObjectPosition(clientID, l_foot, -1, vrep.simx_opmode_streaming) start_time = int(round(time.time())) while vrep.simxGetConnectionId(clientID) != -1 and ((headLocation[2] > 0.0 or headLocation[2] == 0.0) and ((int(round(time.time())) - start_time) < 100)): for i in range(12): returnCode, legPositions[i] = vrep.simxGetJointPosition(clientID, legJoints[i], vrep.simx_opmode_buffer) # do something to the joints dummy right now #remember to convert to radians and back degreeLegPositions = [0,0,0,0,0,0,0,0,0,0,0,0] for i in range(len(legPositions)): degreeLegPositions[i] = legPositions[i] * legJointInv[i] * 180 / math.pi inputData = [] for element in degreeLegPositions: inputData.append(element) newJointPositions = model_iterator.runModel(model,[tuple(inputData)]) newJointPositions = newJointPositions[0] for i in range(len(newJointPositions)): newJointPositions[i] = newJointPositions[i] * math.pi / 180 vrep.simxPauseCommunication(clientID,1) for i in range(12): vrep.simxSetJointTargetPosition(clientID,legJoints[i],newJointPositions[i] * legJointInv[i] * hipJointInv[i],vrep.simx_opmode_oneshot) vrep.simxPauseCommunication(clientID,0) time.sleep(0.001) returncode, headLocation = vrep.simxGetObjectPosition(clientID, head, -1, vrep.simx_opmode_buffer) returnCode, end_r = vrep.simxGetObjectPosition(clientID, r_foot, -1, vrep.simx_opmode_buffer) returnCode, end_l = vrep.simxGetObjectPosition(clientID, l_foot, -1, vrep.simx_opmode_buffer) distanceTraveled = (((end_r[1] + end_l[1])/2) - ((begin_r[1] + begin_l[1])/2)) vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait) time.sleep(0.25) return distanceTraveled
def run_onlyimu_training(sim_number, sim_time):
print("start running onlyIMU model")
prefs.codegen.target = "numpy"
start_scope()
# number of sensor in robot
N_sensor = 8
N_vision = 2
# robot radius
r = 0.0586
# CD neurons number
N_CD = 36
# vision detection_neurons number
N_VD = 10
# HD neuron number
N_HD = 72
# coordinate neuron
N_x_axis, N_y_axis = 32, 32
N_PI = N_x_axis * N_y_axis
# Simulation time
# Speed cell number
N_speed = 6
m = 5 # 60
iter = 0
# width and length of the arena x=width/ y=lenght
# 1 square in vrep = 0.5*0.5
x_scale = 2
y_scale = 2
# distance unit per neuron
N = 200 / (N_x_axis * N_y_axis)
# define the initial mismatch encounter
last_mismatch = 0
##--------------------------------------------------------------------------------------------------------------------##
##--------------------Collision detection Neural Architecture--------------------------------------------##
##--------------------------------------------------------------------------------------------------------------------##
Notred_red_inh, spikemon_red, spikemon_notred, spikemon_estimatedlandmark, spikemon_nonlandmark, wall_landmark_synapse, landmark_wall_synapse, Poisson_group, Poisson_vision, Collision_neuron, Collision_or_not, Color, Wall, Landmark, Poisson_synapse, PoissonVision_synapse, Self_ex_synapse, Self_ex_synapse_color, Self_in_synapse, Self_in_synapse_color, Collide_or_not_synapse, Red_landmark_ex, Red_wall_inh, Notred_landmark_inh, spikemon_CD, spikemon_collision, spikemon_landmark, spikemon_poisson, spikemon_wall, Poisson_non_collision, Non_collision_neuron, Poisson_non_synapse, CON_noncollision_synapse, Non_collision_color_synapses, spikemon_noncollison, Estimated_landmark_neuron, Non_landmark_neuron, Poisson_nonlandmark_synapse, Landmark_nonlandmark_synapse, CON_landmark_ex, CON_wall_ex, Notred_wall_ex, Red, Notred, color_notred_synapses, color_red_synapses = CD_net(
N_CD, N_vision, N_VD)
# gaussian input for 8 collison sensors
stimuli = np.array([
gaussian_spike(N_CD, j * N_CD / N_sensor, 10, 0.2)
for j in range(N_sensor)
])
# gaussion distribution for 2 vision input(wall or landmark)
stimuli_vision = np.array([
gaussian_spike(N_VD, j * N_VD / N_vision, 10, 0.2)
for j in range(N_vision)
])
##--------------------------------------------------------------------------------------------------------------------##
##--------------------Head Direction and position Neural Architecture----------------------------------------------------------##
##--------------------------------------------------------------------------------------------------------------------##
IHD_in_synapse, str_inh_synapse, Poisson_straight, Go_straight_neuron, Go_straight, HD_str_synapse, str_IHD_synapse, sti_PI, sti, Poisson_Left, Poisson_Right, Poisson_Compass, Left_drive, Right_drive, HD_Neuron, IHD_Neuron, Left_shift_neuron, Left_shift_speed_neuron, Right_shift_neuron, Right_shift_speed_neuron, HD_Left_synapse, Left_IHD_synapse, HD_Right_synapse, Left_drive_synapse, Right_drive_synapse, Right_IHD_synapse, HD_IHD_synapse, Reset_synapse, HD_ex_synapse, HD_in_synapse, spikemon_HD, spikemon_IHD, Poisson_PI, PI_Neurons, IPI_Neurons, Directional_Neurons, HD_directional_synapse, directional_PI_synapse, PI_shifting_neurons, North_shifting_neurons, South_shifting_neurons, East_shifting_neurons, West_shifting_neurons, PI_ex_synapse, PI_in_synapse, PI_N_synapse, PI_S_synapse, PI_E_synapse, PI_W_synapse, IPI_N_synapse, IPI_S_synapse, IPI_E_synapse, IPI_W_synapse, IPI_PI_synapse, PI_Reset_synapse, spikemon_PI, spikemon_IPI, NE_shifting_neurons, SE_shifting_neurons, WS_shifting_neurons, WN_shifting_neurons, PI_NE_synapse, PI_SE_synapse, PI_WS_synapse, PI_WN_synapse, IPI_NE_synapse, IPI_SE_synapse, IPI_WS_synapse, IPI_WN_synapse, Left_inh_synapse, Right_IHD_synapse, IPI_in_synapse, IPI_stay_synapse, Stay_stay_layer, Stay_layer, Stay, PI_stay_synapse = HD_PI_integrated_net(
N_HD, N_speed, N_x_axis, N_y_axis, sim_time)
##-----------------head direction correcting matrix network-------------#
# Poisson_IMU,IMU_Neuron,HD_errormatrix_neurons,HD_positive_error,HD_negative_error,IMU_poi_synapses,IMU_errors_connecting_synapses,HD_error_connect_synapses,Error_negative_synapses,Error_positive_synapses,Ex_speed_pool_Neurons,Inh_speed_pool_Neurons,Positive_ex_pool_synapses,Negative_inh_pool_synapses,statemon_positiveHD_error,statemon_negativeHD_error,spikemon_positiveHD_error,spikemon_negativeHD_error,spiketrain_pos=HD_error_correcting(N_HD,HD_Neuron)
##--------------------------------------------------------------------------------------------------------------------##
##--------------------Fusi Synapse between Position - Landmark Neuron----------------------------------------------------------##
##--------------------------------------------------------------------------------------------------------------------##
'''measure collision weight by fusi synapse landmark'''
landmark_PI_plastic = fusi_landmark(PI_Neurons, Landmark)
w_landmark_plas_shape = np.shape(landmark_PI_plastic.w_fusi_landmark)[0]
w_plastic_landmark = landmark_PI_plastic.w_fusi_landmark
all_fusi_weights_landmark = landmark_PI_plastic.w_fusi_landmark
##--------------------------------------------------------------------------------------------------------------------##
##--------------------Fusi Synapse between Position - Wall Neuron----------------------------------------------------------##
##--------------------------------------------------------------------------------------------------------------------##
'''measure collision weight by fusi synapse landmark'''
wall_PI_plastic = fusi_wall(PI_Neurons, Wall)
w_plas_shape_wall = np.shape(wall_PI_plastic.w_fusi_wall)[0]
w_plastic_wall = wall_PI_plastic.w_fusi_wall
all_fusi_weights_wall = wall_PI_plastic.w_fusi_wall
##--------------------------------------------------------------------------------------------------------------------##
##-------------------------------------------Mismatch Network--------------------------------------------------------------##
##--------------------------------------------------------------------------------------------------------------------##
'''store previously estimated landmark position by fusi synapse estimatedlandmark'''
preeq = '''
v_post += 1*(w_piest >= 0.5)
'''
PI_Neurons_est_synapse = Synapses(PI_Neurons,
Estimated_landmark_neuron,
'w_piest : 1',
on_pre=preeq)
PI_Neurons_est_synapse.connect()
Mismatch_landmark_inh_synpase, landmark_mismatch_inh_synapse, spikemon_nonestimatedlandmark, NonEst_landmark_poisson, Non_estimatedlandmark_neuron, Mismatch_neuron, Non_Mismatch_neuron, Nonestimatedlandmark_poi_synapse, Est_nonest_inh_synapse, NonCol_mismatch_synapse, NonCol_nonmismatch_synapse, NonEst_mistmatch_inh_synapse, Est_mismatch_ex_synapse, Est_nonmismatch_inh_synapses, Mismatch_est_inh_synpase, spikemon_Mismatch = mismatch_net(
Non_landmark_neuron, Estimated_landmark_neuron, Landmark)
##--------------------------------------------------------------------------------------------------------------------##
##----------------------------------------------Network--------------------------------------------------------------##
##--------------------------------------------------------------------------------------------------------------------##
print("adding network")
# Network
PINet = Network()
# add collision network
PINet.add(Notred_red_inh, spikemon_red, spikemon_notred,
spikemon_estimatedlandmark, spikemon_nonlandmark,
wall_landmark_synapse, landmark_wall_synapse, Poisson_group,
Poisson_vision, Collision_neuron, Collision_or_not, Color, Wall,
Landmark, Poisson_synapse, PoissonVision_synapse,
Self_ex_synapse, Self_ex_synapse_color, Self_in_synapse,
Self_in_synapse_color, Collide_or_not_synapse, Red_landmark_ex,
Red_wall_inh, Notred_landmark_inh, spikemon_CD,
spikemon_collision, spikemon_landmark, spikemon_poisson,
spikemon_wall, Poisson_non_collision, Non_collision_neuron,
Poisson_non_synapse, CON_noncollision_synapse,
Non_collision_color_synapses, spikemon_noncollison,
Estimated_landmark_neuron, Non_landmark_neuron,
Poisson_nonlandmark_synapse, Landmark_nonlandmark_synapse,
CON_landmark_ex, CON_wall_ex, Notred_wall_ex, Red, Notred,
color_notred_synapses, color_red_synapses)
# add position network
PINet.add(
IHD_in_synapse, str_inh_synapse, Poisson_straight, Go_straight_neuron,
Go_straight, HD_str_synapse, str_IHD_synapse, Poisson_Left,
Poisson_Right, Poisson_Compass, Left_drive, Right_drive, HD_Neuron,
IHD_Neuron, Left_shift_neuron, Left_shift_speed_neuron,
Right_shift_neuron, Right_shift_speed_neuron, HD_Left_synapse,
Left_IHD_synapse, HD_Right_synapse, Left_drive_synapse,
Right_drive_synapse, Right_IHD_synapse, HD_IHD_synapse, Reset_synapse,
HD_ex_synapse, HD_in_synapse, spikemon_HD, spikemon_IHD, Poisson_PI,
PI_Neurons, IPI_Neurons, Directional_Neurons, HD_directional_synapse,
directional_PI_synapse, PI_shifting_neurons, North_shifting_neurons,
South_shifting_neurons, East_shifting_neurons, West_shifting_neurons,
PI_ex_synapse, PI_in_synapse, PI_N_synapse, PI_S_synapse, PI_E_synapse,
PI_W_synapse, IPI_N_synapse, IPI_S_synapse, IPI_E_synapse,
IPI_W_synapse, IPI_PI_synapse, PI_Reset_synapse, spikemon_PI,
spikemon_IPI, NE_shifting_neurons, SE_shifting_neurons,
WS_shifting_neurons, WN_shifting_neurons, PI_NE_synapse, PI_SE_synapse,
PI_WS_synapse, PI_WN_synapse, IPI_NE_synapse, IPI_SE_synapse,
IPI_WS_synapse, IPI_WN_synapse, Left_inh_synapse, Right_IHD_synapse,
IPI_in_synapse, IPI_stay_synapse, Stay_stay_layer, Stay_layer, Stay,
PI_stay_synapse)
# add fusi plastic synapses for landmark,wall neurons with PI neurons
PINet.add(landmark_PI_plastic)
PINet.add(wall_PI_plastic)
# add mismatch network
PINet.add(PI_Neurons_est_synapse, Mismatch_landmark_inh_synpase,
landmark_mismatch_inh_synapse, spikemon_nonestimatedlandmark,
NonEst_landmark_poisson, Non_estimatedlandmark_neuron,
Mismatch_neuron, Non_Mismatch_neuron,
Nonestimatedlandmark_poi_synapse, Est_nonest_inh_synapse,
NonCol_mismatch_synapse, NonCol_nonmismatch_synapse,
NonEst_mistmatch_inh_synapse, Est_mismatch_ex_synapse,
Est_nonmismatch_inh_synapses, Mismatch_est_inh_synpase,
spikemon_Mismatch)
##-----------------------------------------------------------------------------------------##
##---------------------------------Robot controller----------------------------------------##
##-----------------------------------------------------------------------------------------##
# Connect to the server
print('finished adding network')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID != -1:
print("Connected to remote API server")
else:
print("Not connected to remote API server")
sys.exit("Could not connect")
vrep.simxSynchronous(clientID, 1)
##----------------------------------Controller initialized------------------------------------------##
# set motor
err_code, l_motor_handle = vrep.simxGetObjectHandle(
clientID, "KJunior_motorLeft", vrep.simx_opmode_blocking)
err_code, r_motor_handle = vrep.simxGetObjectHandle(
clientID, "KJunior_motorRight", vrep.simx_opmode_blocking)
# Compass output=orientation
# define robot
err_code, robot = vrep.simxGetObjectHandle(clientID, 'KJunior',
vrep.simx_opmode_blocking)
# define Angles
err_code, Angles = vrep.simxGetObjectOrientation(
clientID, robot, -1, vrep.simx_opmode_streaming)
# define object position
err_code, Position = vrep.simxGetObjectPosition(clientID, robot, -1,
vrep.simx_opmode_streaming)
# get sensor
sensor1, sensor2, sensor3, sensor4, sensor5, sensor6, sensor7, sensor8 = getSensor(
clientID)
# read point
detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4, detectedPoint5, detectedPoint6, detectedPoint7, detectedPoint8 = getDetectedpoint(
sensor1, sensor2, sensor3, sensor4, sensor5, sensor6, sensor7, sensor8,
clientID)
# Distance from sensor to obstacle
sensor_val1, sensor_val2, sensor_val3, sensor_val4, sensor_val5, sensor_val6, sensor_val7, sensor_val8 = getSensorDistance(
detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4,
detectedPoint5, detectedPoint6, detectedPoint7, detectedPoint8)
# get sensor for mismatch landmark detection
err_code, sensorMismatch = vrep.simxGetObjectHandle(
clientID, "Proximity_sensor", vrep.simx_opmode_blocking)
err_code, detectionStateMismatch, detectedPointMismatch, detectedObjectHandleMismatch, detectedSurfaceNormalVectorMismatch = vrep.simxReadProximitySensor(
clientID, sensorMismatch, vrep.simx_opmode_streaming)
mismatch_sensordistance = np.linalg.norm(detectedPointMismatch)
# get vision sensor
err_code, camera = vrep.simxGetObjectHandle(clientID, "Vision_sensor",
vrep.simx_opmode_blocking)
err_code, resolution, image = vrep.simxGetVisionSensorImage(
clientID, camera, 1, vrep.simx_opmode_streaming)
##---------------------------------Sensor initial condition (for CD)------------------------------------##
# sensor value of every sensor for every neuron
# Use inv_filter to filter out noise
# sensor_val = np.array([np.repeat(inv_filter(sensor_val1),N_CD), np.repeat(inv_filter(sensor_val2),N_CD),np.repeat(inv_filter(sensor_val3),N_CD),np.repeat(inv_filter(sensor_val7),N_CD),np.repeat(inv_filter(sensor_val8),N_CD)])
sensor_val = np.array([
np.repeat(inv_filter(sensor_val1), N_CD),
np.repeat(inv_filter(sensor_val2), N_CD),
np.repeat(inv_filter(sensor_val3), N_CD),
np.repeat(inv_filter(sensor_val4), N_CD),
np.repeat(inv_filter(sensor_val5), N_CD),
np.repeat(inv_filter(sensor_val6), N_CD),
np.repeat(inv_filter(sensor_val7), N_CD),
np.repeat(inv_filter(sensor_val8), N_CD)
])
sensor_val_vision = np.zeros([N_vision, N_VD])
# sum of each sensor value * its gaussian distribution --> sum to find all activity for each neurons --> WTA
All_stimuli = np.sum(sensor_val * stimuli, axis=0)
All_stimuli_vision = np.sum(sensor_val_vision * stimuli_vision, axis=0)
# find the winner
winner = WTA(All_stimuli)
for w in winner:
Poisson_synapse.w_poi[w] = All_stimuli[w]
winner_vision = WTA(All_stimuli_vision)
for w in winner_vision:
PoissonVision_synapse.w_vision_poi[w] = All_stimuli_vision[w]
##------------------------------------------------------------------------------------------------##
# initial speed of wheel
l_steer = 0.24
r_steer = 0.24
# record the initial time
t_int = time.time()
# record compass angle
compass_angle = np.array([])
# record x and y axis at each time
x_axis = np.array([])
y_axis = np.array([])
# path that is passing
r = np.array([])
# all_time
all_time = np.array([])
# collision index that the robot collided
collision_index = np.array([])
collision_index_during_run = []
# record weight parameter
# record every m sec
weight_record_time = 1
weight_during_run_landmark = np.zeros(
[np.shape(w_plastic_landmark)[0], 3 * int(sim_time / 5)])
weight_during_run_wall = np.zeros(
[np.shape(w_plastic_wall)[0], 3 * int(sim_time / 5)])
time_step_list = np.array([])
# start simulation
while (time.time() - t_int) < sim_time:
t1 = time.time() - t_int
# record proximity sensor at each time step
sensor_val1, sensor_val2, sensor_val3, sensor_val4, sensor_val5, sensor_val6, sensor_val7, sensor_val8 = getSensorDistance(
detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4,
detectedPoint5, detectedPoint6, detectedPoint7, detectedPoint8)
all_sensor = np.array(
[sensor_val1, sensor_val2, sensor_val3, sensor_val4, sensor_val5])
all_sensor[all_sensor < 4.1e-20] = np.infty
activated_sensor = np.argmin(all_sensor)
# obtain vision sensor values
pixelimage = set(image)
if all_sensor[activated_sensor] < 0.1:
if activated_sensor == 3:
r_steer, l_steer, zeta = TurnRight(r_steer, l_steer, delta_t)
elif activated_sensor == 4:
r_steer, l_steer, zeta = TurnRight(r_steer, l_steer, delta_t)
elif activated_sensor == 0:
r_steer, l_steer, zeta = TurnLeft(r_steer, l_steer, delta_t)
elif activated_sensor == 1:
r_steer, l_steer, zeta = TurnLeft(r_steer, l_steer, delta_t)
elif activated_sensor == 2:
r_steer, l_steer, zeta = TurnLeft(r_steer, l_steer, delta_t)
else:
l_steer = 0.24
r_steer = 0.24
zeta = 0
else:
l_steer = 0.24
r_steer = 0.24
zeta = 0
####-------------------- Record weight------------------------####
weight_during_run_landmark[:, iter] = w_plastic_landmark
weight_during_run_wall[:, iter] = w_plastic_wall
collision_index_during_run.append(collision_index)
####-------------------- Start recording spike (CD) ------------------------####
# cleaning noises for the collision distance sensor value
sensor_val = np.array([
np.repeat(inv_filter(sensor_val1), N_CD),
np.repeat(inv_filter(sensor_val2), N_CD),
np.repeat(inv_filter(sensor_val3), N_CD),
np.repeat(inv_filter(sensor_val4), N_CD),
np.repeat(inv_filter(sensor_val5), N_CD),
np.repeat(inv_filter(sensor_val6), N_CD),
np.repeat(inv_filter(sensor_val7), N_CD),
np.repeat(inv_filter(sensor_val8), N_CD)
])
# get the vision sensor value
sensor_val_vision = generate_vision_val(pixelimage, N_VD)
# reset weight to 0 again
Poisson_synapse.w_poi = 0
PoissonVision_synapse.w_vision_poi = 0
err_code = vrep.simxSetJointTargetVelocity(clientID, l_motor_handle,
l_steer,
vrep.simx_opmode_streaming)
err_code = vrep.simxSetJointTargetVelocity(clientID, r_motor_handle,
r_steer,
vrep.simx_opmode_streaming)
# All stimuli and WTA
All_stimuli = (np.sum(sensor_val * stimuli, axis=0))
winner = WTA(All_stimuli)
for w in winner:
Poisson_synapse.w_poi[w] = All_stimuli[w]
All_stimuli_vision = np.sum(sensor_val_vision * stimuli_vision, axis=0)
winner_vision = WTA(All_stimuli_vision)
for w in winner_vision:
PoissonVision_synapse.w_vision_poi[w] = All_stimuli_vision[w] / 10
# --------mismatching detecting---------##
PI_Neurons_est_synapse.w_piest = landmark_PI_plastic.w_fusi_landmark
####-------------------- End recording spike ----------------------------####
####-------------------- Start recording Head direction ------------------------####
# Choose neuron that is the nearest to the turning speed/direction
# if turn left
if r_steer == 0:
neuron_index = nearest_neuron_speed(zeta, N_speed)
for syn in range(N_speed):
Left_drive_synapse[syn].w_left_drive = 0
Right_drive_synapse[syn].w_right_drive = 0
Left_drive_synapse[neuron_index].w_left_drive = 5
Right_drive_synapse[neuron_index].w_right_drive = 0
Go_straight.w_str = -3
directional_PI_synapse.w_dir_PI = 0
Stay_stay_layer.w_stay_stay = 2
# if turn right
elif l_steer == 0:
neuron_index = nearest_neuron_speed(zeta, N_speed)
for syn in range(N_speed):
Left_drive_synapse[syn].w_left_drive = 0
Right_drive_synapse[syn].w_right_drive = 0
Left_drive_synapse[neuron_index].w_left_drive = 0
Right_drive_synapse[neuron_index].w_right_drive = 5
Go_straight.w_str = -3
directional_PI_synapse.w_dir_PI = 0 # if turn position PI stay
Stay_stay_layer.w_stay_stay = 2
# if go straight
else:
for syn in range(N_speed):
Left_drive_synapse[syn].w_left_drive = 0
Right_drive_synapse[syn].w_right_drive = 0
Go_straight.w_str = 10
directional_PI_synapse.w_dir_PI = 4 # if move position PI run
Stay_stay_layer.w_stay_stay = -4
##-----------------reset IHD (Compass) -----------------##
# Get actual heading direction
err_code, Angles = vrep.simxGetObjectOrientation(
clientID, robot, -1, vrep.simx_opmode_streaming)
heading_dir = getHeadingdirection(Angles)
compass_angle = np.append(compass_angle, heading_dir)
# recalibrate head direction to nearest neuron
recal = nearest_neuron_head(heading_dir, N_HD)
# set reset by compass weight upon angle atm
Reset_synapse.w_reset = np.array(gaussian_spike(N_HD, recal, 30, 0.03))
# print("reset synapse",Reset_synapse.w_reset)
##-----------------head direction error correction -----------------##
if len(spikemon_Mismatch) > last_mismatch:
print('encounter mismatch')
err_code, detectionStateMismatch, detectedPointMismatch, detectedObjectHandleMismatch, detectedSurfaceNormalVectorMismatch = vrep.simxReadProximitySensor(
clientID, sensorMismatch, vrep.simx_opmode_streaming)
mismatch_sensordistance = np.linalg.norm(detectedPointMismatch)
while mismatch_sensordistance <= 0.01:
l_steer = 0
r_steer = 0.5
err_code = vrep.simxSetJointTargetVelocity(
clientID, l_motor_handle, l_steer,
vrep.simx_opmode_streaming)
err_code = vrep.simxSetJointTargetVelocity(
clientID, r_motor_handle, r_steer,
vrep.simx_opmode_streaming)
err_code, detectionStateMismatch, detectedPointMismatch, detectedObjectHandleMismatch, detectedSurfaceNormalVectorMismatch = vrep.simxReadProximitySensor(
clientID, sensorMismatch, vrep.simx_opmode_streaming)
mismatch_sensordistance = np.linalg.norm(detectedPointMismatch)
current_error_distance = mismatch_sensordistance
print("mismatch distance", mismatch_sensordistance)
l_steer = 0.24
r_steer = 0.24
err_code = vrep.simxSetJointTargetVelocity(
clientID, l_motor_handle, l_steer, vrep.simx_opmode_streaming)
err_code = vrep.simxSetJointTargetVelocity(
clientID, r_motor_handle, r_steer, vrep.simx_opmode_streaming)
last_mismatch = len(spikemon_Mismatch)
##----------------- Read position -----------------##
err_code, Position = vrep.simxGetObjectPosition(
clientID, robot, -1, vrep.simx_opmode_streaming)
x_pos = Position[0]
y_pos = Position[1]
# recalibrate head direction to nearest neuron
x_axis = np.append(x_axis, x_pos)
y_axis = np.append(y_axis, y_pos)
# recalibrate
recal_x_axis = nearest_neuron_x_axis(x_pos, N_x_axis, x_scale)
recal_y_axis = nearest_neuron_y_axis(y_pos, N_y_axis, y_scale)
recal_index = N_x_axis * recal_y_axis + recal_x_axis
r = np.append(
r, recal_index
) # is an array keeping all index that neuron fire during the run
# set reset weight
##----------------- Recording Position Index when collision -----------------##
if l_steer == 0 or r_steer == 0:
collision_index = np.append(collision_index, recal_index)
##----------------- Collect time-----------------##
all_time = np.append(all_time, time.time() - t_int)
# run
PINet.run(15 * ms)
print("current plas landmark synapese index",
np.where(w_plastic_landmark >= 0.5))
print('wall synpases values index bigger',
np.where(w_plastic_wall >= 0.5))
print(
'####-------------------- Read sensor (new round) ----------------------------####'
)
# slow down the controller for more stability
# time.sleep(4.2)
# Start new measurement in the next time step
detectedPoint1, detectedPoint2, detectedPoint3, detectedPoint4, detectedPoint5, detectedPoint6, detectedPoint7, detectedPoint8 = getDetectedpoint(
sensor1, sensor2, sensor3, sensor4, sensor5, sensor6, sensor7,
sensor8, clientID)
err_code, detectionStateMismatch, detectedPointMismatch, detectedObjectHandleMismatch, detectedSurfaceNormalVectorMismatch = vrep.simxReadProximitySensor(
clientID, sensorMismatch, vrep.simx_opmode_streaming)
err_code, resolution, image = vrep.simxGetVisionSensorImage(
clientID, camera, 1, vrep.simx_opmode_streaming)
# record time step and final time
t2 = time.time() - t_int
delta_t = t2 - t1
final_time = time.time()
time_step_list = np.append(time_step_list, delta_t)
print("delta-t", delta_t, iter)
print("final time t2", int(t2))
iter += 1
##---------------end of simulation ---------------##
print("Done")
# saving data
# setting pathway
pathway = "/Users/jieyab/SNN/for_plotting/"
exp_number = str(sim_number)
np.save(pathway + "r" + exp_number, r)
np.save(pathway + "spikemon_CD_i" + exp_number, spikemon_CD.i)
np.save(pathway + "spikemon_CD_t" + exp_number, spikemon_CD.t / ms)
np.save(pathway + "spikemon_HD_i" + exp_number, spikemon_HD.i)
np.save(pathway + "spikemon_HD_t" + exp_number, spikemon_HD.t / ms)
np.save(pathway + "spikemon_PI_i" + exp_number, spikemon_PI.i)
np.save(pathway + "spikemon_PI_t" + exp_number, spikemon_PI.t / ms)
np.save(pathway + "spikemon_noncollision_i" + exp_number,
spikemon_noncollison.i)
np.save(pathway + "spikemon_noncollision_t" + exp_number,
spikemon_noncollison.t / ms)
np.save(pathway + "spikemon_wall_i" + exp_number, spikemon_wall.i)
np.save(pathway + "spikemon_wall_t" + exp_number, spikemon_wall.t / ms)
np.save(pathway + "spikemon_nonlandmark_i" + exp_number,
spikemon_nonlandmark.i)
np.save(pathway + "spikemon_nonlandmark_t" + exp_number,
spikemon_nonlandmark.t / ms)
np.save(pathway + "spikemon_landmark_i" + exp_number, spikemon_landmark.i)
np.save(pathway + "spikemon_landmark_t" + exp_number,
spikemon_landmark.t / ms)
np.save(pathway + "weight_during_run_landmark" + exp_number,
weight_during_run_landmark)
np.save(pathway + "weight_during_run_wall" + exp_number,
weight_during_run_wall)
np.save(pathway + "weight_landmark" + exp_number, w_plastic_landmark)
np.save(pathway + "weight__wall" + exp_number, w_plastic_wall)
np.save(pathway + "spikemon_mismatch_t" + exp_number,
spikemon_Mismatch.t / ms)
np.save(pathway + "spikemon_mismatch_v" + exp_number, spikemon_Mismatch.v)
np.save(pathway + "collision_index" + exp_number, collision_index)
np.save(pathway + "collision_index_during_run" + exp_number,
collision_index_during_run)
np.save(pathway + "compass_angle" + exp_number, compass_angle)
np.save(pathway + "all_time" + exp_number, all_time)
np.save(pathway + "spikemon_estimatedlandmark_i" + exp_number,
spikemon_estimatedlandmark.i)
np.save(pathway + "spikemon_estimatedlandmark_t" + exp_number,
spikemon_estimatedlandmark.t / ms)
np.save(pathway + "spikemon_nonestimatedlandmark_i" + exp_number,
spikemon_nonestimatedlandmark.i)
np.save(pathway + "spikemon_nonestimatedlandmark_t" + exp_number,
spikemon_nonestimatedlandmark.t / ms)
np.save(pathway + "spikemon_red_i" + exp_number, spikemon_red.i)
np.save(pathway + "spikemon_red_t" + exp_number, spikemon_red.t / ms)
np.save(pathway + "spikemon_notred_i" + exp_number, spikemon_notred.i)
np.save(pathway + "spikemon_notred_t" + exp_number, spikemon_notred.t / ms)
np.save(pathway + "step_time" + exp_number, time_step_list)
return clientID
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]]
def main():
# global variables
global velocity
global armJoints
global wheels
global MAX_FORCE
global tcp_handle
global bodyHandle
global proxSensor
global holdingCube
# get transformation data
M = transformation_data.M
S = transformation_data.S
T_b_s = transformation_data.T_b_s
zero_pose = transformation_data.zero_pose
plate_pose = transformation_data.plate_pose
front_pose = transformation_data.front_pose
# VREP stuff
print('Program 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 remote API server')
# gets handle for TCP - used in printing comparison
e, tcp_handle = vrep.simxGetObjectHandle(clientID,
'youBotGripperJoint1',
vrep.simx_opmode_oneshot_wait)
e, bodyHandle = vrep.simxGetObjectHandle(clientID, "youBot",
vrep.simx_opmode_blocking)
e, cubeHandle = vrep.simxGetObjectHandle(clientID, "Rectangle16",
vrep.simx_opmode_blocking)
e, proxSensor = vrep.simxGetObjectHandle(clientID, "Proximity_sensor",
vrep.simx_opmode_blocking)
#e, visionSensorHandle = vrep.simxGetObjectHandle(clientID, "Vision_sensor", vrep.simx_opmode_blocking)
# initialize wheel motors
e1, wheels[0] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_fl',
vrep.simx_opmode_oneshot_wait)
e2, wheels[1] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_rl',
vrep.simx_opmode_oneshot_wait)
e3, wheels[2] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_rr',
vrep.simx_opmode_oneshot_wait)
e4, wheels[3] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_fr',
vrep.simx_opmode_oneshot_wait)
#initialize arm joints
arm_poses = [0, 0, 0, 0, 0]
for i in range(5):
# get object handle
e, armJoints[i] = vrep.simxGetObjectHandle(
clientID, 'youBotArmJoint' + str(i),
vrep.simx_opmode_oneshot_wait)
# set max force
vrep.simxSetJointForce(clientID, armJoints[i], MAX_FORCE,
vrep.simx_opmode_oneshot_wait)
# MAKE A CALL WITH simx_opmode_streaming TO INIT DATA AQUISITION
vrep.simxGetObjectPosition(clientID, bodyHandle, -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectOrientation(clientID, bodyHandle, -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectPosition(clientID, bodyHandle, -1,
vrep.simx_opmode_buffer)
vrep.simxGetObjectOrientation(clientID, bodyHandle, -1,
vrep.simx_opmode_buffer)
#IMAGE PROCESSING
res, vsHandle = vrep.simxGetObjectHandle(clientID, 'Vision_sensor',
vrep.simx_opmode_oneshot_wait)
res, bsHandle = vrep.simxGetObjectHandle(clientID, 'Has_Block_sensor',
vrep.simx_opmode_oneshot_wait)
res, cubeHandle = vrep.simxGetObjectHandle(
clientID, 'Rectangle14', vrep.simx_opmode_oneshot_wait)
err, resolution, image = vrep.simxGetVisionSensorImage(
clientID, vsHandle, 0, vrep.simx_opmode_streaming)
err, resolution, image = vrep.simxGetVisionSensorImage(
clientID, bsHandle, 0, vrep.simx_opmode_streaming)
#TESTING if we can move the arm between two poses
'''
DO NOT DELETE
NEEDED TO MAKE ROBOT GET CORRECT POSITION AT START (wait until buffer is cleared)
'''
print("Initializing robot...\n\n")
time.sleep(2)
stopWheels()
moveArm(transformation_data.plate_pose, [])
release()
### INSERT YOUR CODE HERE ###
# MEANS NO CUBE -0.046000331640244
'''
while True:
grab()
release()
'''
continue_running = True
while continue_running: # This will happen as long as the robot is alive
print("Start of main loop")
start_time = time.time()
# STATE 1
#Search for a cube
dist, blob_center = getCubeProperties(clientID, vsHandle)
while (dist < 0):
turnRight(0) #spins
#print(dist)
dist, blob_center = getCubeProperties(clientID, vsHandle)
if (time.time() - start_time >
10): #If we've been searching for 15 seconds
#Give up
continue_running = False
break
stopWheels()
if (not continue_running): #no cubes found by front camera
print("Giving up...")
break
# STATE 2
#Navigate to cube
# TODO: instead of drifting, we want rotation so that it is a lot more natural
# could look at the move to position function
print("Navigating to cube")
while (len(blob_center) != 0 and
blob_center[0] > 140): # MOVE RIGHT UNTIL BLOCK IS CENTERED
moveRight()
dist, blob_center = getCubeProperties(clientID, vsHandle)
while (len(blob_center) != 0 and
blob_center[0] < 110): # MOVE LEFT UNTIL BLOCK IS CENTERED
moveLeft()
dist, blob_center = getCubeProperties(clientID, vsHandle)
while (len(blob_center) != 0 and blob_center[1] < 90
): # MOVE FORWARD UNTIL BLOCK IS WITHIN REACH
moveForward()
dist, blob_center = getCubeProperties(clientID, vsHandle)
stopWheels()
# STATE 3
#Grab cube
print("Grabbing cube")
detectionState, yaw, cube_pose = detectCube()
grabCube_success = grabCube(cube_pose, yaw)
# Check if we have cube
if (grabCube_success):
time.sleep(0.5)
#print('---------Start Debugging!!!!!!!!---------')
dist, blob_center = getRearCubeProperties(clientID, bsHandle)
if (len(blob_center) == 0):
grabCube_success = False
print("Successfully grabbed cube: " + str(grabCube_success))
#STATE 4
#Move to destination
if (grabCube_success):
returnCube()
release()
time.sleep(5)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
print('Program ended')
def getPosition(self, objectHandle):
return vrep.simxGetObjectPosition(self.clientID, objectHandle, -1,
vrep.simx_opmode_oneshot)
def position(self):
return vrep.unwrap_vrep(
vrep.simxGetObjectPosition(self._clientID, self._Robobo, -1, vrep.simx_opmode_blocking)
)
print 'Python program started'
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19998, True, True, 5000,
5) #Timeout=5000ms, Threadcycle=5ms
if clientID != -1:
print 'Connected to V-REP'
else:
print 'Failed connecting to V-REP'
vrep.simxFinish(clientID)
if clientID != -1:
#ball position
ErrorCode, ballPos = vrep.simxGetObjectHandle(
clientID, 'Ball', vrep.simx_opmode_oneshot_wait)
ErrorCode, ballXYZ = vrep.simxGetObjectPosition(clientID, ballPos, -1,
vrep.simx_opmode_streaming)
#ErrorCode, ballPos = vrep.simxGetObjectPosition (clientID,ball,-1,vrep.simx_opmode_streaming)
#ErrorCode, ballPos = vrep.simxGetObjectPosition (clientID,ball,-1,vrep.simx_opmode_buffer)
#motors here
ErrorCode, LeftJointHandle = vrep.simxGetObjectHandle(
clientID, 'Blue1_leftJoint',
vrep.simx_opmode_oneshot_wait) #Left motor
ErrorCode, RightJointHandle = vrep.simxGetObjectHandle(
clientID, 'Blue1_rightJoint',
vrep.simx_opmode_oneshot_wait) #Right motor
ErrorCode = vrep.simxSetJointTargetVelocity(clientID, LeftJointHandle, 0,
vrep.simx_opmode_oneshot_wait)
ErrorCode = vrep.simxSetJointTargetVelocity(clientID, RightJointHandle, 0,
vrep.simx_opmode_oneshot_wait)
print("Erro em obter o handle")
# # Armazena o posicionamento angular dos sensores
# sensor_angle = np.array([PI/2, 50/180.0*PI, 30/180.0*PI, 10/180.0*PI,
# -10/180.0*PI, -30/180.0*PI, -50/180.0*PI, -PI/2, -PI/2,
# -130/180.0*PI, -150/180.0*PI, -170/180.0*PI, 170/180.0*PI,
# 150/180.0*PI, 130/180.0*PI, PI/2])
# Posicionamento angular dos sensores
sensor_angle = np.array([
90, 50, 30, 10, -10, -30, -50, -90, -90, -130, -150, -170, 170, 150,
130, 90
])
sensor_angle = sensor_angle * PI / 180.0
# recupera a posição do robô
returnCode, position = vrep.simxGetObjectPosition(
clientID, robot_handle, -1, vrep.simx_opmode_streaming)
returnCode, eulerAngles = vrep.simxGetObjectOrientation(
clientID, robot_handle, -1, vrep.simx_opmode_streaming)
startTime = time.time()
sensor_handle_list = []
for i in range(16):
errCode, sensor_handle = vrep.simxGetObjectHandle(
clientID, 'Pioneer_p3dx_ultrasonicSensor' + str(i + 1),
vrep.simx_opmode_oneshot_wait)
sensor_handle_list.append(sensor_handle)
errCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor(
clientID, sensor_handle, vrep.simx_opmode_streaming)
def position(self):
err_code, position = vrep.simxGetObjectPosition(
self.simulation.clientID, self.robotnik, -1,
vrep.simx_opmode_buffer)
return position
print(instruction[0])
if(instruction[0] == 1 or instruction[0] == 5):
print("contraer pierna")
elif(instruction[0] == 2 or instruction[0] == 6):
print("Estirar pierna")
elif(instruction[0] == 3 or instruction[0] == 7):
print("pierna hacia adelante")
elif(instruction[0] == 4 or instruction[0] == 8):
print("Pierna hacia atrás")
#This is what makes the simulation Synchronous
initialTime = vrep.simxGetLastCmdTime(clientID)
actualTime = initialTime
#Condition to make a new movement
while(actualTime - initialTime < instruction[1]/10):
#Retrive head position
headPosition = vrep.simxGetObjectPosition(clientID, head, -1, vrep.simx_opmode_oneshot)
headTrace.append(headPosition)
#print(headPosition)
#print(contador, " ", actualTime, " ", initialTime)#, " ",actualTime-initialTime," - ", instruction[1]," - ",contador,vrep.simxGetObjectPosition(clientID, head, -1, vrep.simx_opmode_oneshot))
#Advance time in my internal counter
actualTime+=dt
#Make de simulation run one step (dt detemines how many seconds pass by between step and step)
vrep.simxSynchronousTrigger(clientID)
'''
TODO Tengo que cambiar el sleep por controlar el tiempo en le simulación
Entonces voy a hacer un loop infinito en el cual pregunto el tiempo,
luego guardo el tiempo y la diferencia entre el t inicial y el actual
debe ser igual o mayor que el de la la próxima instrucción.
De esa manera entre vuelta y vuelta del while que espera a que
pase el tiempo voy a poder preguntar por las posiciones de la
errorCode,P_handle=vrep.simxGetObjectHandle(clientID,'P',vrep.simx_opmode_oneshot_wait)
errorCode,L_handle=vrep.simxGetObjectHandle(clientID,'L',vrep.simx_opmode_oneshot_wait)
errorCode,R_handle=vrep.simxGetObjectHandle(clientID,'R',vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetVelocity(clientID,P_handle,0,vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetVelocity(clientID,L_handle,0,vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetVelocity(clientID,R_handle,0,vrep.simx_opmode_oneshot_wait)
def speed(handle,speed):
errorCode = vrep.simxSetJointTargetVelocity(clientID,handle,speed,vrep.simx_opmode_oneshot_wait)
vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait)
while True:
errorCode,position_S=vrep.simxGetObjectPosition(clientID,Sphere_handle,-1,vrep.simx_opmode_oneshot)
if position_S[1] <= 0.195 and position_S[1] >0.075 and position_S[0]<-0.154 and position_S[0]>-0.194:
speed(L_handle,R_KickBallVel)
else:
speed(L_handle,B_KickBallVel)
if position_S[1] <= 0.195 and position_S[1] >0.075 and position_S[0]<-0.1 and position_S[0]>-0.121:
speed(R_handle,B_KickBallVel)
else:
speed(R_handle,R_KickBallVel)
if position_S[0]>-0.0226 and position_S[0]<-0.0176 and position_S[1]<0.0250 and position_S[1]>=0.0212:
speed(P_handle,-1500)
else:
speed(P_handle,1000)
def get_end_pose(self):
pose = vrep.simxGetObjectPosition(self.cid, self.end_handle, -1,
vrep.simx_opmode_blocking)[1]
return np.array(pose)
# Start the simulation:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait)
# ???
res, U4 = vrep.simxGetObjectHandle(clientID, 'J11R',
vrep.simx_opmode_blocking)
res, U5 = vrep.simxGetObjectHandle(clientID, 'shaft19',
vrep.simx_opmode_blocking)
res, robotHandle = vrep.simxGetObjectHandle(clientID, 'bodyDyn',
vrep.simx_opmode_blocking)
res, endHandle = vrep.simxGetObjectHandle(clientID, 'Destination',
vrep.simx_opmode_blocking)
res, robotPosition = vrep.simxGetObjectPosition(
clientID=clientID,
objectHandle=robotHandle,
relativeToObjectHandle=endHandle,
operationMode=vrep.simx_opmode_oneshot_wait)
# camera
res, kinect_depth_camera = vrep.simxGetObjectHandle(
clientID, 'kinect_depth', vrep.simx_opmode_blocking)
res, kinect_rgb_camera = vrep.simxGetObjectHandle(
clientID, 'kinect_rgb', vrep.simx_opmode_blocking)
res, kinect_joint = vrep.simxGetObjectHandle(clientID, 'kinect_joint',
vrep.simx_opmode_blocking)
#Retrive the body coordinate system
# coordinate ????
res, GCS = vrep.simxGetObjectHandle(clientID, 'GCS',
vrep.simx_opmode_blocking)
def setUp(self):
'''////////// CONNECT //////////'''
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID = vrep.simxStart('127.0.0.1', 19998, True, True, 5000,
5) # Connect to V-REP
if self.clientID != -1:
pass
#print ('Connected to remote API server')
else:
print('Connection Failed')
sys.exit('Could not connect')
self.done = False
'''////////// HANDLE //////////'''
_, self.goal = vrep.simxGetObjectHandle(self.clientID, 'Goal',
vrep.simx_opmode_oneshot_wait)
_, self.robot = vrep.simxGetObjectHandle(self.clientID, 'Omnirob',
vrep.simx_opmode_oneshot_wait)
for i in range(0, 4):
self.motor_handle.append(
vrep.simxGetObjectHandle(
self.clientID,
'Omnirob_' + self.motor_name[i] + 'wheel_motor',
vrep.simx_opmode_oneshot_wait)[1])
for i in range(0, 9 + 1):
self.cube_handle.append(
vrep.simxGetObjectHandle(self.clientID, 'Cuboid' + str(i),
vrep.simx_opmode_oneshot_wait)[1])
for i in range(0, 18):
self.sensor_handle.append(
vrep.simxGetObjectHandle(self.clientID, 'sensor' + str(i + 1),
vrep.simx_opmode_oneshot_wait)[1])
'''////////// SET POSITION & ANGLE //////////'''
vrep.simxSetObjectOrientation(self.clientID, self.robot, -1,
[0, 0, math.pi],
vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(
self.clientID, self.goal, -1,
[0, 0, random.uniform(0, 2 * math.pi)], vrep.simx_opmode_oneshot)
self.l = []
self.randomPos()
z = random.randint(0, 10)
z = -10.25 if z in [1, 5, 8, 9] else 0.25
vrep.simxSetObjectPosition(self.clientID, self.robot, -1,
[self.l[10][1], self.l[10][2], 0.28],
vrep.simx_opmode_oneshot)
vrep.simxSetObjectPosition(self.clientID, self.goal, -1,
[self.l[11][1], self.l[11][2], 0.00],
vrep.simx_opmode_oneshot)
for i in range(0, 9 + 1):
vrep.simxSetObjectPosition(self.clientID, self.cube_handle[i], -1,
[self.l[i][1], self.l[i][2], z],
vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(
self.clientID, self.cube_handle[i], -1,
[0, 0, random.uniform(0, 2 * math.pi)],
vrep.simx_opmode_oneshot)
_, self.velocity_robot, self.angvelocity_robot = vrep.simxGetObjectVelocity(
self.clientID, self.robot, -1)
_, self.position_robot = vrep.simxGetObjectPosition(
self.clientID, self.robot, -1, vrep.simx_opmode_streaming)
_, self.position_goal = vrep.simxGetObjectPosition(
self.clientID, self.goal, -1, vrep.simx_opmode_streaming)
_, self.angle_robot = vrep.simxGetObjectOrientation(
self.clientID, self.robot, -1, vrep.simx_opmode_streaming)
'''////////// START //////////'''
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_continuous)
self.pre_distance2goal = 0.0
def setGBS():
res, BCS_position = vrep.simxGetObjectPosition(
clientID, BCS, -1, vrep.simx_opmode_oneshot_wait)
[x, y, z] = [BCS_position[0], BCS_position[1], BCS_position[2]]
vrep.simxSetObjectPosition(clientID, GCS, -1, [x, y, 0],
vrep.simx_opmode_oneshot_wait)
gain_divisor = 0.5 * (np.dot(d_lm.T, (m * d_lm - g))) + 1e-10
gain = grain_numerator / gain_divisor
if gain > 0:
self.vector = vector_new
m = m * np.max([1 / 3, 1 - (2 * gain - 1)**3])
else:
m = m * v
v = v * 2
i += 1
t1 = time.time()
print('i:', i, ' time:', t1 - t0, ' F:', self.F0(self.vector))
# print self.vector[8] + 1.0389e-1
stew = Platform()
i = 0
while True:
i += 0.05
stew.move(0.2 * sin(i) + 0.2)
pos = vrep.simxGetObjectPosition(client_id, pos_h, -1,
vrep.simx_opmode_oneshot)
stew.get_position()
print('error:', stew.vector[8] - pos[1][2] + 1.0389e-1)
def train(self):
vrep.simxFinish(-1) #clean up the previous stuff
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID == -1:
print("Could not connect to server")
sys.exit()
first = True
for i_episode in range(100):
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
observation1 = robot1.observation_space
observation2 = robot2.observation_space
ep_r = 0
self.steps = 0
while True:
action1 = self.RL1.choose_action(observation1) # To check
# print(action1)
observation1_, done1 = robot1.step(action1) # To check
#print(observation1_)
observation2_ = robot2.observation_space
done2 = False
x1, y1, z1, vx1, vy1, vz1, theta1_f, theta2_f, theta3_f = observation1_ # To check
x2, y2, z2, vx2, vy2, vz2, theta1_b, theta2_b, theta3_b = observation2_
error, self.r1 = vrep.simxGetObjectHandle(
clientID, 'body#1', vrep.simx_opmode_blocking)
error, self.r2 = vrep.simxGetObjectHandle(
clientID, 'body#13', vrep.simx_opmode_blocking)
error, self.o1 = vrep.simxGetObjectHandle(
clientID, 'Obstacle#1', vrep.simx_opmode_blocking)
error, position_object = vrep.simxGetObjectPosition(
clientID, self.o1, -1, vrep.simx_opmode_blocking)
x1_ = position_object[0]
y1_ = position_object[1]
z1_ = position_object[2]
error, position_hexa_base1 = vrep.simxGetObjectPosition(
clientID, self.r1, -1, vrep.simx_opmode_blocking)
x1 = position_hexa_base1[0]
y1 = position_hexa_base1[1]
z1 = position_hexa_base1[2]
error, position_hexa_base2 = vrep.simxGetObjectPosition(
clientID, self.r2, -1, vrep.simx_opmode_blocking)
x2 = position_hexa_base2[0]
y2 = position_hexa_base2[1]
z2 = position_hexa_base2[2]
distance = np.sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) *
(y1 - y2))
distance_object = np.sqrt((x1 - x1_) * (x1 - x1_) +
(y1 - y1_) * (y1 - y1_) +
(z1 - z1_) * (z1 - z1_))
###########################
if distance > 1 or distance < 0.15 or distance_object < 0.3:
done1 = True
######################################
#########reward function############
reward1 = self.rsrvl + (
vx1 + vx2) - 0.5 * (np.abs(vy1) + np.abs(vy2))
reward = 1 * (distance < 0.15) - 10 * (
distance > 1 or
distance_object < 0.3) - 0.1 * self.steps - 0.1 * distance
#################################
#print("R: ", reward)
print("distance: ", distance)
print("distance_object", distance_object)
# print("z1:",z1)
self.RL1.store_transition(observation1, action1, reward,
observation1_)
if self.total_steps > 200 and self.total_steps % 5 == 0:
self.RL1.learn()
ep_r += reward
if done1:
#print(done1)
print('episode: ', i_episode, 'ep_r: ', round(ep_r, 2),
' epsilon: ', round(self.RL1.epsilon, 2))
break
observation1 = observation1_
observation2 = observation2_
self.total_steps += 1
self.steps += 1
done1 = False
first = False
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
time.sleep(1)
self.RL1.plot_cost()
raise Exception('could not get object handle for whelel')
result, wheel4 = vrep.simxGetObjectHandle(clientID,
'OmniWheel_regularRotation#2',
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for whelel')
# ==================================================================================================== #
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
# ******************************** Your robot control code goes here ******************************** #
SetJointPosition(np.array([0, 0, 0, 0, 0, 0]))
result, p1 = vrep.simxGetObjectPosition(clientID, joint_one_handle, car,
vrep.simx_opmode_blocking)
result, p2 = vrep.simxGetObjectPosition(clientID, joint_two_handle, car,
vrep.simx_opmode_blocking)
result, p3 = vrep.simxGetObjectPosition(clientID, joint_three_handle, car,
vrep.simx_opmode_blocking)
result, p4 = vrep.simxGetObjectPosition(clientID, joint_four_handle, car,
vrep.simx_opmode_blocking)
result, p5 = vrep.simxGetObjectPosition(clientID, joint_five_handle, car,
vrep.simx_opmode_blocking)
result, p6 = vrep.simxGetObjectPosition(clientID, joint_six_handle, car,
vrep.simx_opmode_blocking)
result, r1 = vrep.simxGetObjectOrientation(clientID, joint_one_handle, car,
vrep.simx_opmode_blocking)
result, r2 = vrep.simxGetObjectOrientation(clientID, joint_two_handle, car,
vrep.simx_opmode_blocking)
result, r3 = vrep.simxGetObjectOrientation(clientID, joint_three_handle, car,
def getPosition(self):
ret, position = vrep.simxGetObjectPosition(self.clientID, self.robotHandle, -1, vrep.simx_opmode_oneshot)
return position
print('Program 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 remote API server')
input = []
output = []
#ping pong ball
#initialization of ball
res, ball0 = vrep.simxGetObjectHandle(clientID, 'Sphere',
vrep.simx_opmode_blocking)
res, ballpos = vrep.simxGetObjectPosition(clientID, ball0, -1,
vrep.simx_opmode_streaming)
time.sleep(1)
res, ballOrigin = vrep.simxGetObjectPosition(clientID, ball0, -1,
vrep.simx_opmode_buffer)
time.sleep(1)
res, xval = vrep.simxGetFloatSignal(clientID, "xtarget",
vrep.simx_opmode_streaming)
res, yval = vrep.simxGetFloatSignal(clientID, "ytarget",
vrep.simx_opmode_streaming)
res, zval = vrep.simxGetFloatSignal(clientID, "ztarget",
vrep.simx_opmode_streaming)
res, xdest = vrep.simxGetFloatSignal(clientID, "xdest",
vrep.simx_opmode_streaming)
res, ydest = vrep.simxGetFloatSignal(clientID, "ydest",
def getState():
#height
res, handle = vrep.simxGetObjectHandle(clientID, 'LHipYawPitch3',
vrep.simx_opmode_blocking)
res, pos = vrep.simxGetObjectPosition(clientID, handle, -1,
vrep.simx_opmode_blocking)
height = pos[2]
print("initial height:", height)
res, handlhead = vrep.simxGetObjectHandle(clientID,
"HeadPitch_link_respondable",
vrep.simx_opmode_blocking)
#torso
res, handle = vrep.simxGetObjectHandle(clientID, "imported_part_20_sub0",
vrep.simx_opmode_blocking)
#res, Angle = vrep.simxGetObjectOrientation(clientID, handle, handlhead, vrep.simx_opmode_blocking)
res, Angle = vrep.simxGetObjectOrientation(clientID, handle, -1,
vrep.simx_opmode_blocking)
res, Torsolv, Torsoav = vrep.simxGetObjectVelocity(
clientID, handle, vrep.simx_opmode_blocking)
res, Torsopos = vrep.simxGetObjectPosition(clientID, handle, -1,
vrep.simx_opmode_blocking)
TorsoAngle = Angle[2] / 180 * 3.14
#Rthigh
res, handle = vrep.simxGetObjectHandle(clientID, "imported_part_13_sub",
vrep.simx_opmode_blocking)
#res, Angle = vrep.simxGetObjectOrientation(clientID, handle, handlhead, vrep.simx_opmode_blocking)
res, Angle = vrep.simxGetObjectOrientation(clientID, handle, -1,
vrep.simx_opmode_blocking)
res, Rthighlv, Rthighav = vrep.simxGetObjectVelocity(
clientID, handle, vrep.simx_opmode_blocking)
res, Rthighpos = vrep.simxGetObjectPosition(clientID, handle, -1,
vrep.simx_opmode_blocking)
RthighAngle = Angle[2] / 180 * 3.14
#Lthigh
res, handle = vrep.simxGetObjectHandle(clientID, "imported_part_10_sub",
vrep.simx_opmode_blocking)
#res, Angle = vrep.simxGetObjectOrientation(clientID, handle, handlhead, vrep.simx_opmode_blocking)
res, Angle = vrep.simxGetObjectOrientation(clientID, handle, -1,
vrep.simx_opmode_blocking)
res, Lthighlv, Lthighav = vrep.simxGetObjectVelocity(
clientID, handle, vrep.simx_opmode_blocking)
res, Lthighpos = vrep.simxGetObjectPosition(clientID, handle, -1,
vrep.simx_opmode_blocking)
LthighAngle = Angle[2] / 180 * 3.14
#Rcalf
res, handle = vrep.simxGetObjectHandle(clientID, "r_knee_pitch_link_pure2",
vrep.simx_opmode_blocking)
#res, Angle = vrep.simxGetObjectOrientation(clientID, handle, handlhead, vrep.simx_opmode_blocking)
res, Angle = vrep.simxGetObjectOrientation(clientID, handle, -1,
vrep.simx_opmode_blocking)
res, Rcalflv, Rcalfav = vrep.simxGetObjectVelocity(
clientID, handle, vrep.simx_opmode_blocking)
res, Rcalfpos = vrep.simxGetObjectPosition(clientID, handle, -1,
vrep.simx_opmode_blocking)
RcalfAngle = Angle[2] / 180 * 3.14
#Lcalf
res, handle = vrep.simxGetObjectHandle(clientID, "l_knee_pitch_link_pure2",
vrep.simx_opmode_blocking)
#res, Angle = vrep.simxGetObjectOrientation(clientID, handle, handlhead, vrep.simx_opmode_blocking)
res, Angle = vrep.simxGetObjectOrientation(clientID, handle, -1,
vrep.simx_opmode_blocking)
res, Lcalflv, Lcalfav = vrep.simxGetObjectVelocity(
clientID, handle, vrep.simx_opmode_blocking)
res, Lcalfpos = vrep.simxGetObjectPosition(clientID, handle, -1,
vrep.simx_opmode_blocking)
LcalfAngle = Angle[2] / 180 * 3.14
state = [height]
state.append(TorsoAngle)
state.extend(Torsolv)
state.extend(Torsoav)
state.extend(Torsopos)
state.append(RthighAngle)
state.extend(Rthighlv)
state.extend(Rthighav)
state.extend(Rthighpos)
state.append(LthighAngle)
state.extend(Lthighlv)
state.extend(Lthighav)
state.extend(Lthighpos)
state.append(RcalfAngle)
state.extend(Rcalflv)
state.extend(Rcalfav)
state.extend(Rcalfpos)
state.append(LcalfAngle)
state.extend(Lcalflv)
state.extend(Lcalfav)
state.extend(Lcalfpos)
state = np.array(state)
return state
errorCode, left_motor_handle_2 = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor#2',
vrep.simx_opmode_blocking)
print("Car 1 leftJoint", left_motor_handle_2, errorCode)
errorCode, right_motor_handle_2 = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor#2',
vrep.simx_opmode_blocking)
print("Car 1 rightJoint", right_motor_handle_2, errorCode)
# init sensors
sensors_0_dist = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
sensors_1_dist = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
sensors_2_dist = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
for i in range(16):
errorCode, sensors_0_dist[i] = vrep.simxGetFloatSignal(clientID, 'Sensor_0_{}'.format(i), vrep.simx_opmode_streaming)
#init gps
errorCode, car0_pos = vrep.simxGetObjectPosition(clientID, car_0, -1, vrep.simx_opmode_streaming)
errorCode, car0_dir = vrep.simxGetObjectOrientation(clientID, car_0, -1, vrep.simx_opmode_streaming)
errorCode, car1_pos = vrep.simxGetObjectPosition(clientID, car_1, -1, vrep.simx_opmode_streaming)
errorCode, car1_dir = vrep.simxGetObjectOrientation(clientID, car_1, -1, vrep.simx_opmode_streaming)
errorCode, car2_pos = vrep.simxGetObjectPosition(clientID, car_2, -1, vrep.simx_opmode_streaming)
errorCode, car2_dir = vrep.simxGetObjectOrientation(clientID, car_2, -1, vrep.simx_opmode_streaming)
# sample codes
v0 = 2
vLeft_0 = v0
vRight_0 = v0
vLeft_1 = v0
vRight_1 = v0
vLeft_2 = v0
def init():
global robot
global blockHandleArray, connectionTime
print('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
int_portNb = 19999 # define port_nr
clientID = vrep.simxStart('127.0.0.1', int_portNb, True, True, 5000,
5) # connect to server
if clientID != -1:
print('Connected to remote API server')
res, objs = vrep.simxGetObjects(
clientID, vrep.sim_handle_all,
vrep.simx_opmode_oneshot_wait) # get all objects in the scene
if res == vrep.simx_return_ok: # Remote function call succeeded
print('Number of objects in the scene: ',
len(objs)) # print number of object in the scene
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_sl, ultraSonicSensorLeft = vrep.simxGetObjectHandle(
clientID, 'Pioneer_p3dx_ultrasonicSensor3',
vrep.simx_opmode_oneshot_wait)
ret_sr, ultraSonicSensorRight = vrep.simxGetObjectHandle(
clientID, 'Pioneer_p3dx_ultrasonicSensor5',
vrep.simx_opmode_oneshot_wait)
blockHandleArray = []
for i_block in range(12):
blockName = 'ConcretBlock#' + str(i_block)
retCode, handle = vrep.simxGetObjectHandle(
clientID, blockName, vrep.simx_opmode_oneshot_wait)
assert retCode == 0, retCode
if i_block > 6:
rand_loc = [
random.random() * 6 - 1.5,
random.random() * 7 - 2.5, 0.0537
] # x[-1.5,4.5] y[-2.5,-4.5]
vrep.simxSetObjectPosition(clientID, handle, -1, rand_loc,
vrep.simx_opmode_oneshot_wait)
retCode, position = vrep.simxGetObjectPosition(
clientID, handle, -1, vrep.simx_opmode_oneshot_wait)
assert retCode == 0, retCode
blockHandleArray.append([handle, i_block, position])
robot = EasyDict(clientID=clientID,
leftMotorHandle=leftMotorHandle,
rightMotorHandle=rightMotorHandle,
pioneerRobotHandle=pioneerRobotHandle,
ultraSonicSensorLeft=ultraSonicSensorLeft,
ultraSonicSensorRight=ultraSonicSensorRight,
energySensor=None)
connectionTime = vrep.simxGetLastCmdTime(robot.clientID)
return robot
else:
print('Remote API function call returned with error code: ', res)
vrep.simxFinish(clientID) # close all opened connections
else:
print('Failed connecting to remote API server')
print('Program finished')
return {}
'ResizableFloor_5_25',
vrep.simx_opmode_blocking)
jointHandles.append(joint1)
jointHandles.append(joint2)
jointHandles.append(joint3)
while True:
try:
vrep.simxPauseCommunication(clientID, 1)
i = i + 1
#Get Robot State
if i == 1:
errorCode, eulerAngles = vrep.simxGetObjectOrientation(
clientID, body, -1, vrep.simx_opmode_streaming)
errorCode, position = vrep.simxGetObjectPosition(
clientID, body, -1, vrep.simx_opmode_streaming)
else:
errorCode, eulerAngles = vrep.simxGetObjectOrientation(
clientID, body, -1, vrep.simx_opmode_buffer)
errorCode, position = vrep.simxGetObjectPosition(
clientID, body, -1, vrep.simx_opmode_buffer)
#print('p1:'+str(position[0])+' p2:'+str(position[1])+' p3:'+str(position[2]))
#print('p1:'+str(eulerAngles[0])+' p2:'+str(eulerAngles[1])+' p3:'+str(eulerAngles[2]))
#PID Control
# position_control = PID_Position.control(0.00005*i,position[0],1)
#
# if(eulerAngles[1]<(3*3.1416/180)):
# orientation_control = PID_Orientation.control(position_control,eulerAngles[1],1)
# elif(eulerAngles[1]>=(3*3.1416/180)):
def main():
# Start V-REP connection
try:
vrep.simxFinish(-1)
print("Connecting to simulator...")
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID == -1:
print("Failed to connect to remote API Server")
vrep_exit(clientID)
except KeyboardInterrupt:
vrep_exit(clientID)
return
# Setup V-REP simulation
print("Setting simulator to async mode...")
vrep.simxSynchronous(clientID, True)
dt = .001
vrep.simxSetFloatingParameter(
clientID,
vrep.sim_floatparam_simulation_time_step,
dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
# Load V-REP scene
print("Loading scene...")
vrep.simxLoadScene(clientID, scene_name, 0xFF, vrep.simx_opmode_blocking)
# Get quadrotor handle
err, quad_handle = vrep.simxGetObjectHandle(clientID, quad_name,
vrep.simx_opmode_blocking)
# Initialize quadrotor position and orientation
vrep.simxGetObjectPosition(clientID, quad_handle, -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectOrientation(clientID, quad_handle, -1,
vrep.simx_opmode_streaming)
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
# Initialize rotors
print("Initializing propellers...")
for i in range(len(propellers)):
vrep.simxClearFloatSignal(clientID, propellers[i],
vrep.simx_opmode_oneshot)
# Get quadrotor initial position and orientation
err, pos_old = vrep.simxGetObjectPosition(clientID, quad_handle, -1,
vrep.simx_opmode_buffer)
err, euler_old = vrep.simxGetObjectOrientation(clientID, quad_handle, -1,
vrep.simx_opmode_buffer)
pos_old = np.asarray(pos_old)
euler_old = np.asarray(euler_old) * 10.
pos_start = pos_old
# hyper parameters
n_input = 6
n_forces = 4
#n_action = 8
hidden = 256
batch_size = 512
learning_rate = .01
eps = 0.1
gamma = 0.9
init_f = 7.
net = DQN(n_input + n_forces, hidden, 1)
optim = torch.optim.Adam(net.parameters(), lr=learning_rate)
state = [0 for i in range(n_input)]
state = torch.from_numpy(np.asarray(state,
dtype=np.float32)).view(-1, n_input)
propeller_vels = [init_f, init_f, init_f, init_f]
extended_state = torch.cat(
(state, torch.from_numpy(np.asarray([propeller_vels],
dtype=np.float32))), 1)
memory = ReplayMemory(10000)
while (vrep.simxGetConnectionId(clientID) != -1):
# epsilon greedy
r = random.random()
if r > eps:
delta_forces = generate_forces(net, extended_state, learning_rate)
else:
delta_forces = [float(random.randint(-1, 1)) for i in range(4)]
# Set propeller thrusts
print("Setting propeller thrusts...")
propeller_vels = apply_forces(propeller_vels, delta_forces)
# Send propeller thrusts
print("Sending propeller thrusts...")
[
vrep.simxSetFloatSignal(clientID, prop, vels,
vrep.simx_opmode_oneshot)
for prop, vels in zip(propellers, propeller_vels)
]
# Trigger simulator step
print("Stepping simulator...")
vrep.simxSynchronousTrigger(clientID)
# Get quadrotor initial position and orientation
err, pos_new = vrep.simxGetObjectPosition(clientID, quad_handle, -1,
vrep.simx_opmode_buffer)
err, euler_new = vrep.simxGetObjectOrientation(clientID, quad_handle,
-1,
vrep.simx_opmode_buffer)
pos_new = np.asarray(pos_new)
euler_new = np.asarray(euler_new) * 10
#euler_new[2]/=100
valid = is_valid_state(pos_start, pos_new, euler_new)
if valid:
next_state = torch.FloatTensor(
np.concatenate([euler_new, pos_new - pos_old]))
#next_state = torch.FloatTensor(euler_new )
next_extended_state = torch.FloatTensor(
[np.concatenate([next_state,
np.asarray(propeller_vels)])])
else:
next_state = None
next_extended_state = None
reward_acc = np.float32(-np.linalg.norm(
pos_new - pos_old)) if next_state is not None else np.float32(-10.)
reward_alpha = np.float32(
-np.fabs(euler_new[0])
) if not check_quad_flipped(euler_new) else np.float32(-10.)
reward_beta = np.float32(
-np.fabs(euler_new[1])
) if not check_quad_flipped(euler_new) else np.float32(-10.)
reward_gamma = np.float32(
-np.fabs(euler_new[2])
) if not check_quad_flipped(euler_new) else np.float32(-5.)
reward_alpha_acc = np.float32(
-np.fabs(euler_new[0] - euler_old[0])
) if not check_quad_flipped(euler_new) else np.float32(-10.)
reward_beta_acc = np.float32(
-np.fabs(euler_new[1] - euler_old[1])
) if not check_quad_flipped(euler_new) else np.float32(-10.)
reward_gamma_acc = np.float32(
-np.fabs(euler_new[2] - euler_old[2])
) if not check_quad_flipped(euler_new) else np.float32(-5.)
reward = reward_alpha + reward_beta + reward_gamma + reward_acc + reward_alpha_acc * 10. + reward_beta_acc * 10. + reward_gamma_acc * 10.
if not check_quad_flipped(euler_new) and not valid:
print('Success.')
reward = 10.
#reward_pos=-np.linalg.norm(pos_new-pos_start)
if reward < -10.:
reward = -10.
reward = np.float32(reward)
memory.push(
extended_state,
torch.from_numpy(np.asarray([delta_forces], dtype=np.float32)),
next_extended_state, torch.from_numpy(np.asarray([[reward]])))
state = next_state
extended_state = next_extended_state
pos_old = pos_new
euler_old = euler_new
print(propeller_vels)
print("\n")
# Perform one step of the optimization (on the target network)
#DQN_update2(net, memory, batch_size, gamma, optim)
if not valid:
DQN_update2(net, memory, batch_size, gamma, optim)
print('reset')
# reset
reset(clientID)
print("Loading scene...")
vrep.simxLoadScene(clientID, scene_name, 0xFF,
vrep.simx_opmode_blocking)
# Setup V-REP simulation
print("Setting simulator to async mode...")
vrep.simxSynchronous(clientID, True)
dt = .0005
vrep.simxSetFloatingParameter(
clientID,
vrep.sim_floatparam_simulation_time_step,
dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
# Get quadrotor handle
err, quad_handle = vrep.simxGetObjectHandle(
clientID, quad_name, vrep.simx_opmode_blocking)
# Initialize quadrotor position and orientation
vrep.simxGetObjectPosition(clientID, quad_handle, -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectOrientation(clientID, quad_handle, -1,
vrep.simx_opmode_streaming)
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
# Get quadrotor initial position and orientation
err, pos_old = vrep.simxGetObjectPosition(clientID, quad_handle,
-1,
vrep.simx_opmode_buffer)
err, euler_old = vrep.simxGetObjectOrientation(
clientID, quad_handle, -1, vrep.simx_opmode_buffer)
euler_old = euler_old
pos_old = np.asarray(pos_old)
euler_old = np.asarray(euler_old) * 10.
pos_start = pos_old
state = [0 for i in range(n_input)]
state = torch.FloatTensor(np.asarray(state)).view(-1, n_input)
propeller_vels = [init_f, init_f, init_f, init_f]
extended_state = torch.cat(
(state, torch.FloatTensor(np.asarray([propeller_vels]))), 1)
print('duration: ', len(memory))
memory = ReplayMemory(10000)
def get_position(self):
position = vrep.simxGetObjectPosition(
self.client_id, self.handles['youBot_positionTarget'],
-1, BLOCKING_MODE
)[1]
return position
print('Connected to remote API server')
#获得相机句柄
_, sensorHandle = vrep.simxGetObjectHandle(clientID, 'Vision_sensor#',
vrep.simx_opmode_oneshot_wait)
_, sphereHandle = vrep.simxGetObjectHandle(clientID, 'Plane',
vrep.simx_opmode_oneshot_wait)
#获得深度相机的近景和远景距离
_, nearClip = vrep.simxGetObjectFloatParameter(
clientID, sensorHandle, vrep.sim_visionfloatparam_near_clipping,
vrep.simx_opmode_oneshot_wait)
_, farClip = vrep.simxGetObjectFloatParameter(
clientID, sensorHandle, vrep.sim_visionfloatparam_far_clipping,
vrep.simx_opmode_oneshot_wait)
print("farclip:%s nearclip:%s" % (farClip, nearClip))
#深度相机位置
_, sensorPostion = vrep.simxGetObjectPosition(
clientID, sensorHandle, -1, vrep.simx_opmode_oneshot_wait)
#发送流传送命令
if IMG_TYPE == IMG_RGB:
_, resolution, image = vrep.simxGetVisionSensorImage(
clientID, sensorHandle, 0, vrep.simx_opmode_streaming)
else:
_, resolution, image = vrep.simxGetVisionSensorImage(
clientID, sensorHandle, 1, vrep.simx_opmode_streaming)
time.sleep(2)
while (vrep.simxGetConnectionId(clientID) != -1):
#参数2,options为1时表示灰度图,为0时RGB图,官方教程错误
err, resolution, img = vrep.simxGetVisionSensorImage(
clientID, sensorHandle, 0, vrep.simx_opmode_buffer)
if err == vrep.simx_return_ok:
if IMG_TYPE == IMG_RGB: #RGB图
img = np.array(img, dtype=np.uint8)
