def __init__(self, clientid):
self.clientID = clientid
res, self.rollJoint = vrep.simxGetObjectHandle(
self.clientID, 'RJ', vrep.simx_opmode_blocking)
res, self.gripperJoint1 = vrep.simxGetObjectHandle(
self.clientID, 'SJ1', vrep.simx_opmode_blocking)
res, self.gripperJoint2 = vrep.simxGetObjectHandle(
self.clientID, 'SJ2', vrep.simx_opmode_blocking)
vrep.simxSetJointPosition(self.clientID, self.rollJoint, 0,
vrep.simx_opmode_streaming)
vrep.simxSetJointPosition(self.clientID, self.gripperJoint1, 0,
vrep.simx_opmode_streaming)
vrep.simxSetJointPosition(self.clientID, self.gripperJoint2, 0,
vrep.simx_opmode_streaming)
res, pos1 = vrep.simxGetJointPosition(self.clientID,
self.gripperJoint1,
vrep.simx_opmode_streaming)
res, pos2 = vrep.simxGetJointPosition(self.clientID,
self.gripperJoint2,
vrep.simx_opmode_streaming)
res, pos3 = vrep.simxGetJointPosition(self.clientID, self.rollJoint,
vrep.simx_opmode_streaming)
def send_gear_commands(clientID, target_degree1, target_degree2, gearHandle1,
gearHandle2):
RAD = 180 / math.pi
lastCmdTime = vrep.simxGetLastCmdTime(clientID)
vrep.simxSynchronousTrigger(clientID)
count = 0
while vrep.simxGetConnectionId(clientID) != -1:
count += 1
if count > 20:
break
currCmdTime = vrep.simxGetLastCmdTime(clientID)
dt = currCmdTime - lastCmdTime
ret, gear_pos1 = vrep.simxGetJointPosition(clientID, gearHandle1,
vrep.simx_opmode_buffer)
ret, gear_pos2 = vrep.simxGetJointPosition(clientID, gearHandle2,
vrep.simx_opmode_buffer)
degree1 = round(gear_pos1 * RAD, 2)
degree2 = round(gear_pos2 * RAD, 2)
print(degree1, degree2)
if abs(degree1 - target_degree1) < 0.5 and abs(degree2 -
target_degree2) < 0.5:
break
vrep.simxPauseCommunication(clientID, True)
vrep.simxSetJointTargetPosition(clientID, gearHandle1,
target_degree1 / RAD,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, gearHandle2,
target_degree2 / RAD,
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
lastCmdTime = currCmdTime
vrep.simxSynchronousTrigger(clientID)
def run(self):
global left_wheel_angle, right_wheel_angle, pos_x, pos_y, angle
_, position = vrep.simxGetObjectPosition(self.clientID,
self.robot_handle, -1,
vrep.simx_opmode_buffer)
_, eulerAngles = vrep.simxGetObjectOrientation(self.clientID,
self.robot_handle, -1,
vrep.simx_opmode_buffer)
_, new_right_wheel_angle = vrep.simxGetJointPosition(
self.clientID, self.right_motor_handle, vrep.simx_opmode_streaming)
_, new_left_wheel_angle = vrep.simxGetJointPosition(
self.clientID, self.left_motor_handle, vrep.simx_opmode_streaming)
pos_x = position[0]
pos_y = position[1]
angle = eulerAngles[2]
new_left_wheel_angle = (new_left_wheel_angle + 2 * PI) % (2 * PI)
new_right_wheel_angle = (new_right_wheel_angle + 2 * PI) % (2 * PI)
left_wheel_angle = new_left_wheel_angle
right_wheel_angle = new_right_wheel_angle
while True:
self.update()
time.sleep(0.1)
def step(self, action):
action = np.clip(action, *self.action_bound)
self.jointConfig1 = np.zeros((self.jointNum, ))
self.jointConfig2 = np.zeros((self.jointNum, ))
for i in range(self.jointNum):
_, jpos = vrep.simxGetJointPosition(self.clientID,
self.robot1_jointHandle[i],
vrep.simx_opmode_blocking)
self.jointConfig1[i] = jpos
for i in range(self.jointNum):
_, jpos = vrep.simxGetJointPosition(self.clientID,
self.robot2_jointHandle[i],
vrep.simx_opmode_blocking)
self.jointConfig2[i] = jpos
# print(self.jointConfig1)
self.jointConfig1 += action[:3] * self.dt
self.jointConfig2 += action[-3:] * self.dt
# self.jointConfig1 %= np.pi*2
# self.jointConfig2 %= np.pi*2
self.jointConfig1 = np.clip(self.jointConfig1, *self.joint_bound)
self.jointConfig2 = np.clip(self.jointConfig2, *self.joint_bound)
self.moveto(self.jointConfig1, self.jointConfig2)
# time.sleep(0.1)
s_ = self.getState_v1()
r = self._r_func()
return s_, r, self.get_point, self.get_obstacles
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 open_hand(
self): #this should work to place gripper at initial open position
_, dist = vrep.simxGetJointPosition(self.client_id, self.motor_handle, \
vrep.simx_opmode_blocking)
vrep.simxSetJointForce(self.client_id, self.motor_handle, 20,
vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(self.client_id, self.motor_handle, -0.5, \
vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
# start_time = time.time()
while dist > -1e-06: #8.22427227831e-06:#-1.67e-06: # Block until gripper is fully open
_, dist = vrep.simxGetJointPosition(self.client_id, self.motor_handle, \
vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(self.client_id, self.motor_handle, -0.5, \
vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
# end_time = time.time()
# if (end_time-start_time > 2):
# self.restart()
vrep.simxSetJointTargetVelocity(self.client_id, self.motor_handle, 0.0, \
vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
def __init__(self, clientID):
'''
get handles
set initial state
'''
self.clientID = clientID
self.jointHandles = [-1, -1, -1, -1]
#self.collisionHandles = [-1,-1]
#self.state = [False,False]
jointInd = [0, 1, 2, 3]
jointName = ['uarm_motor' + str(i + 1) for i in range(4)]
self.jointDict = dict(zip(jointInd, jointName))
# joint handle
for i in range(4):
res, self.jointHandles[i] = vrep.simxGetObjectHandle(
clientID, self.jointDict[i], vrep.simx_opmode_blocking)
if res != 0:
print('Failed to find: {}'.format(self.jointDict[i]))
# collision handle
# for i in range(2):
# self.returnCode,self.collisionHandles[i] = vrep.simxGetCollisionHandle(clientID,'Collision'+str(i+1),vrep.simx_opmode_blocking)
print('handles get')
# initialize
for i in range(4):
vrep.simxGetJointPosition(self.clientID, self.jointHandles[i],
vrep.simx_opmode_streaming)
print('initialzied')
def on_press(key):
try:
if key == keyboard.Key.left:
velEsq = -0.5
velDir = 0.5
elif key == keyboard.Key.right:
velEsq = 0.5
velDir = -0.5
elif key == keyboard.Key.space:
velEsq = 0
velDir = 0
elif key == keyboard.Key.esc:
# Stop listener
sys.exit(0)
return False
vrep.simxSetJointTargetVelocity(clientID, rightMotorHandle, velDir,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, leftMotorHandle, velEsq,
vrep.simx_opmode_streaming)
thetaDir = vrep.simxGetJointPosition(clientID, rightMotorHandle,
vrep.simx_opmode_streaming)[1]
thetaEsq = vrep.simxGetJointPosition(clientID, leftMotorHandle,
vrep.simx_opmode_streaming)[1]
localizacao.setAngulos(thetaDir, thetaEsq)
except AttributeError:
print('special key {0} pressed'.format(key))
def testGripper(gripper_handle, gripperID, gripper_Pos, clientID):
time.sleep(1)
# Close the gripper
# vrep.simxSetJointTargetPosition(clientID, gripper_handle, -gripper_Pos, vrep.simx_opmode_oneshot)
# time.sleep(1)
result, theta = vrep.simxGetJointPosition(clientID, gripper_handle,
vrep.simx_opmode_blocking)
print("Theta is {}".format(theta))
# Open the gripper
vrep.simxSetJointTargetPosition(clientID, gripper_handle, theta - np.pi,
vrep.simx_opmode_oneshot)
time.sleep(1)
result, theta = vrep.simxGetJointPosition(clientID, gripper_handle,
vrep.simx_opmode_blocking)
print("Theta is {}".format(theta))
# Hold the gripper
vrep.simxSetJointTargetPosition(clientID, gripper_handle, 0,
vrep.simx_opmode_oneshot)
time.sleep(1)
def wait_till_stream(self):
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, ori = vrep.simxGetObjectOrientation(self.client_id, self.body_handle, -1, vrep.simx_opmode_buffer)
self.logger.info("orient:%s" % str(ori))
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetObjectPosition(self.client_id, self.body_handle, -1, vrep.simx_opmode_buffer)
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetJointPosition(self.client_id, self.left_joint, vrep.simx_opmode_buffer)
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetJointPosition(self.client_id, self.right_joint, vrep.simx_opmode_buffer)
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetObjectFloatParameter(self.client_id, self.right_joint, 2012, vrep.simx_opmode_buffer)
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetObjectFloatParameter(self.client_id, self.right_joint, 2012, vrep.simx_opmode_buffer)
def __init__(self, client_id, name):
self.client_id = client_id
self.name = name
err, self.handle = vrep.simxGetObjectHandle(
client_id, name, vrep.simx_opmode_oneshot_wait)
vrep.simxGetJointPosition(self.client_id, self.handle,
vrep.simx_opmode_streaming)
def reset(self):
self.get_point = False
self.grab_counter = 0
self.get_obstacles = False
self.obstacles_counter = 0
_ = vrep.simxSetObjectPosition(self.clientID,self.ball1Handle, -1,self.pos1 , vrep.simx_opmode_oneshot)
_ = vrep.simxSetObjectPosition(self.clientID,self.ball2Handle , -1,self.pos2 , vrep.simx_opmode_oneshot)
# for i in range(self.jointNum):
# vrep.simxSetJointPosition(self.clientID,self.robot1_jointHandle[i], self.config1[i], vrep.simx_opmode_oneshot)
# for i in range(self.jointNum):
# vrep.simxSetJointPosition(self.clientID,self.robot2_jointHandle[i], self.config2[i],vrep.simx_opmode_oneshot)
time.sleep(0.1)
jointConfig1 = np.zeros((self.jointNum,))
jointConfig2 = np.zeros((self.jointNum,))
for i in range(self.jointNum):
_, jpos = vrep.simxGetJointPosition(self.clientID, self.robot1_jointHandle[i], vrep.simx_opmode_blocking)
jointConfig1[i] = jpos
for i in range(self.jointNum):
_, jpos = vrep.simxGetJointPosition(self.clientID, self.robot2_jointHandle[i], vrep.simx_opmode_blocking)
jointConfig2[i] = jpos
s = np.hstack((jointConfig1, jointConfig2))
_, end1 = vrep.simxGetObjectPosition(self.clientID, self.end1_Handle,self.base1Handle, vrep.simx_opmode_blocking)
_, end2 = vrep.simxGetObjectPosition(self.clientID, self.end2_Handle,self.base2Handle, vrep.simx_opmode_blocking)
del end1[2]
del end2[2]
end1 = np.array(end1)
end2 = np.array(end2)
d1 = self.goal_1-end1
d2 = self.goal_2-end2
s = np.hstack((s, np.hstack((d1,d2))))
return s
def virar(angulo): thetaInicial = localizacao.getOrientacao() print thetaInicial if(angulo > 0): #motorDir.rotate((int) (ang * (DISTANCIA_RODAS / RAIO_RODA))); v_Left = -0.5 v_Right = 0.5 while(abs(thetaInicial - localizacao.getOrientacao()) < abs(angulo)): vrep.simxSetJointTargetVelocity(clientID, rightMotorHandle, v_Right, vrep.simx_opmode_streaming) vrep.simxSetJointTargetVelocity(clientID, leftMotorHandle, v_Left, vrep.simx_opmode_streaming) thetaDir = vrep.simxGetJointPosition(clientID, rightMotorHandle, vrep.simx_opmode_streaming)[1] thetaEsq = vrep.simxGetJointPosition(clientID, leftMotorHandle, vrep.simx_opmode_streaming)[1] localizacao.setAngulos(thetaDir, thetaEsq) else: v_Left = 0.5 v_Right = -0.5 while(abs(thetaInicial - localizacao.getOrientacao()) < abs(angulo)): vrep.simxSetJointTargetVelocity(clientID, rightMotorHandle, v_Right, vrep.simx_opmode_streaming) vrep.simxSetJointTargetVelocity(clientID, leftMotorHandle, v_Left, vrep.simx_opmode_streaming) thetaDir = vrep.simxGetJointPosition(clientID, rightMotorHandle, vrep.simx_opmode_streaming)[1] thetaEsq = vrep.simxGetJointPosition(clientID, leftMotorHandle, vrep.simx_opmode_streaming)[1] localizacao.setAngulos(thetaDir, thetaEsq)
def retrieve_state(self):
pos = np.zeros(2)
vel = np.zeros(2)
tip = np.zeros(3)
_, pos[0] = vrep.simxGetJointPosition(self.clientID,
self.jointHandles[0],
vrep.simx_opmode_blocking)
_, pos[1] = vrep.simxGetJointPosition(self.clientID,
self.jointHandles[1],
vrep.simx_opmode_blocking)
_, vel[0] = vrep.simxGetObjectFloatParameter(self.clientID,
self.jointHandles[0],
2012,
vrep.simx_opmode_blocking)
_, vel[1] = vrep.simxGetObjectFloatParameter(self.clientID,
self.jointHandles[1],
2012,
vrep.simx_opmode_blocking)
_, tip_list = vrep.simxGetObjectPosition(self.clientID, self.tipHandle,
-1, vrep.simx_opmode_blocking)
tip = np.asarray(tip_list)
return {1: pos, 2: vel, 3: tip}
def startDataStreaming():
if (not clientID):
print('connect first')
elif (clientID[0] == -1):
print('connect first')
else:
#Position
for i in Objects_handle:
vrep.simxGetObjectPosition(clientID[0], i, UR3_handle[0],
vrep.simx_opmode_streaming)
#Dummy position
vrep.simxGetObjectPosition(clientID[0], end_effector[0], UR3_handle[0],
vrep.simx_opmode_streaming)
#vision
for i in vision_sensor:
vrep.simxGetVisionSensorImage(clientID[0], i, 0,
vrep.simx_opmode_streaming)
#collision
for i in collision_handle:
vrep.simxReadCollision(clientID[0], i, vrep.simx_opmode_streaming)
#UR3 Joint
for i in UR3_joint_handle:
vrep.simxGetJointPosition(clientID[0], i,
vrep.simx_opmode_streaming)
def run_sim_data(subject, clientID): vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait) # start #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] #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] 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) headLocation = [1,1,1]; returncode, headLocation = vrep.simxGetObjectPosition(clientID, head, -1, vrep.simx_opmode_streaming) for i in range(12): returnCode, legPositions[i] = vrep.simxGetJointPosition(clientID, legJoints[i], vrep.simx_opmode_streaming) #RhipX,RhipY,RhipZ,LhipX,LhipY,LhipZ,RKneeZ,LKneeZ,RAnkleX,RAnkleZ,LAnkleX,LAnkleZ start_time = int(round(time.time())) while vrep.simxGetConnectionId(clientID) != -1 and ((headLocation[2] > 0.3 or headLocation[2] == 0.0) and ((int(round(time.time())) - start_time) < 100)): for j in range(((subject * 101)),((subject * 101) + 202)): for i in range(12): returnCode, legPositions[i] = vrep.simxGetJointPosition(clientID, legJoints[i], vrep.simx_opmode_buffer) joint_data = model_iterator.getActual(j) newJointPositions = [0,0,0,0,0,0,0,0,0,0,0,0] for i in range(len(newJointPositions)): joint_data[i] = joint_data[i] * math.pi / 180 newJointPositions[i] = joint_data[i] * legJointInv[i] * hipJointInv[i] print(j) print(newJointPositions) print() vrep.simxPauseCommunication(clientID,1) for i in range(12): vrep.simxSetJointTargetPosition(clientID,legJoints[i],newJointPositions[i],vrep.simx_opmode_oneshot) vrep.simxPauseCommunication(clientID,0) time.sleep(0.01) vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait) time.sleep(0.25) return
def getState_v1(self): #版本二的状态定义，维度是14
jointConfig1 = np.zeros((self.jointNum, ))
jointConfig2 = np.zeros((self.jointNum, ))
for i in range(self.jointNum):
_, jpos = vrep.simxGetJointPosition(self.clientID,
self.robot1_jointHandle[i],
vrep.simx_opmode_blocking)
jointConfig1[i] = jpos
for i in range(self.jointNum):
_, jpos = vrep.simxGetJointPosition(self.clientID,
self.robot2_jointHandle[i],
vrep.simx_opmode_blocking)
jointConfig2[i] = jpos
s = np.hstack((jointConfig1, jointConfig2))
_, pos1 = vrep.simxGetObjectPosition(self.clientID, self.end1_Handle,
-1, vrep.simx_opmode_blocking)
_, pos2 = vrep.simxGetObjectPosition(self.clientID, self.end2_Handle,
-1, vrep.simx_opmode_blocking)
del pos1[2]
del pos2[2]
pos1 = np.array(pos1)
pos2 = np.array(pos2)
pos = np.hstack((pos1, pos2))
s = np.hstack((s, pos))
_, d1 = vrep.simxReadDistance(self.clientID, self.robot1_goal_Handle,
vrep.simx_opmode_blocking)
_, d2 = vrep.simxReadDistance(self.clientID, self.robot2_goal_Handle,
vrep.simx_opmode_blocking)
s = np.hstack((s, np.array([d1, d2])))
# collision1, collision2 = self.getCollisonStates()
# danger = np.array([1. if collision1 else 0., 1. if collision2 else 0.])
# s = np.hstack((s, danger))
return s
def on_for_degrees(self,
left_speed, right_speed, degrees,
brake=True, block=True):
if brake is not True:
print(f'{__class__.__name__} break not implemented')
if block is not True:
print(f'{__class__.__name__} block not implemented')
# deg to rad
if degrees == 'full':
degrees = 360
if degrees == 'half':
degrees = 180
if degrees == 'right':
degrees = 90
rad = degrees * math.pi / 180
_clientID = self.motors['Left motor port'].kwargs['clientID']
_left_motor = self.motors['Left motor port']
_right_motor = self.motors['Right motor port']
left_speed, right_speed = self._unpack_speeds_to_native_units(
left_speed, right_speed
)
rCL, ini_pos_L = _left_motor.initialize_motor()
rCR, ini_pos_R = _right_motor.initialize_motor()
if VERBOSE:
print(ini_pos_L, ini_pos_R)
pos_L, pos_R = ini_pos_L, ini_pos_R
while ((abs(pos_L - ini_pos_L) <= rad)
and (abs(pos_R - ini_pos_R) <= rad)):
err_vL = vrep.simxSetJointTargetVelocity(
_clientID, _left_motor.kwargs.get('motor'), left_speed,
vrep.simx_opmode_oneshot_wait
)
err_vR = vrep.simxSetJointTargetVelocity(
_clientID, _right_motor.kwargs.get('motor'), right_speed,
vrep.simx_opmode_oneshot_wait
)
rC, pos_L = vrep.simxGetJointPosition(
_clientID,
_left_motor.kwargs.get('motor'),
vrep.simx_opmode_streaming)
rC, pos_R = vrep.simxGetJointPosition(
_clientID,
_right_motor.kwargs.get('motor'),
vrep.simx_opmode_streaming)
if VERBOSE:
print(pos_L - ini_pos_L, pos_R - ini_pos_R)
self.motors['Left motor port'].full_stop()
self.motors['Right motor port'].full_stop()
def connect(self, address='127.0.0.1', port=19999):
vrep.simxFinish(-1) # just in case, close all opened connections
self._clientID = vrep.simxStart(address, port, True, True, 5000, 5) # Connect to V-REP
if self._clientID >= 0: # and clientID_0 != -1:
print('Connected to remote API server: client id {}'.format(self._clientID))
else:
raise VREPCommunicationError('Failed connecting to remote API server')
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._vrep_get_ping_time()
return self
def final_move(clientID, jointsdict, joint_list, collision_library, arm):
if arm == "left":
handle = jointsdict['Baxter_leftArm_joint1']['Joint Handler']
res, theta_i = vrep.simxGetJointPosition(clientID, handle,
vrep.simx_opmode_blocking)
elif arm == "right":
handle = jointsdict['Baxter_rightArm_joint1']['Joint Handler']
res, theta_i = vrep.simxGetJointPosition(clientID, handle,
vrep.simx_opmode_blocking)
return
def deinitGripper(self):
res, pos = vrep.simxGetJointPosition(self.clientID, self.rollJoint,
vrep.simx_opmode_buffer)
while pos < 0:
vrep.simxSetJointPosition(self.clientID, self.rollJoint,
pos + math.pi / 50,
vrep.simx_opmode_oneshot)
res, pos = vrep.simxGetJointPosition(self.clientID, self.rollJoint,
vrep.simx_opmode_buffer)
time.sleep(0.01)
def search_object_2():
print('Searching...')
vrep.simxSetStringSignal(clientID, 'IKCommands', 'LookPos',
vrep.simx_opmode_oneshot) # "Looking" position
center_res = 10
tic_so = time.clock()
objectFound = False
while objectFound is False:
toc_so = time.clock()
errorCode2, resolution, image = vrep.simxGetVisionSensorImage(
clientID, cam_Handle, 0, vrep.simx_opmode_buffer)
im = np.array(image, dtype=np.uint8)
if len(resolution) > 0:
im.resize([resolution[0], resolution[1], 3])
obj_list = image_process_2(im, resolution)
if len(obj_list) > 0:
print(obj_list[0])
if len(obj_list[0]) > 1:
im_x = obj_list[0][1]
im_y = obj_list[0][2]
print('Object Found!')
full_center(im_x, im_y, center_res, resolution)
return 1
errorCode, joint_ang_0 = vrep.simxGetJointPosition(
clientID, joint_handles[0], vrep.simx_opmode_buffer)
errorCode, joint_ang_4 = vrep.simxGetJointPosition(
clientID, joint_handles[4], vrep.simx_opmode_buffer)
if joint_ang_0 > 70 * math.pi / 180:
vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], 0,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, joint_handles[4], -0.2,
vrep.simx_opmode_streaming)
if joint_ang_4 < 100 * math.pi / 180:
vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], -0.2,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, joint_handles[4], 0,
vrep.simx_opmode_streaming)
if joint_ang_0 < -70 * math.pi / 180:
vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], 0,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, joint_handles[4], 0.2,
vrep.simx_opmode_streaming)
if joint_ang_4 > 118 * math.pi / 180:
vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], 0.2,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, joint_handles[4], 0,
vrep.simx_opmode_streaming)
if toc_so - tic_so > 20:
return 6
def run360():
print "connecting to vrep with address", IPADDRESS, "on port", PORT
clientID = vrep.simxStart(IPADDRESS, PORT, True, True, 5000, 5)
print "hello vrep! ", clientID
if clientID == -1:
print "failed to connect to vrep server"
return
# get the motor joint handle
error, jointHandle = vrep.simxGetObjectHandle(
clientID, "plantStandMotor", vrep.simx_opmode_oneshot_wait)
print "starting the motor..."
# set the velocity of the joint
vrep.simxSetJointTargetVelocity(clientID, jointHandle, 1.0,
vrep.simx_opmode_oneshot_wait)
print "spinning 360 degrees..."
# set up to stream joint positions
vrep.simxGetJointPosition(clientID, jointHandle,
vrep.simx_opmode_streaming)
passed90Degrees = False
# The control loop:
while vrep.simxGetConnectionId(
clientID) != -1: # while we are connected to the server..
(error, position) = vrep.simxGetJointPosition(clientID, jointHandle,
vrep.simx_opmode_buffer)
# Fetch the newest joint value from the inbox (func. returns immediately (non-blocking)):
if error == vrep.simx_error_noerror:
# here we have the newest joint position in variable jointPosition!
# break when we've done a 360
if passed90Degrees and position >= 0 and position < math.pi / 2.0:
break
elif not passed90Degrees and position > math.pi / 2.0:
passed90Degrees = True
print "stoppping..."
vrep.simxSetJointTargetVelocity(clientID, jointHandle, 0.0,
vrep.simx_opmode_oneshot_wait)
if clientID != -1:
vrep.simxFinish(clientID)
print "done!"
def signal_values(self):
vrep.simxGetObjectOrientation(self.client_id, self.body_handle, -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectPosition(self.client_id, self.body_handle, -1,
vrep.simx_opmode_streaming)
for handle_idx in range(0, self.joint_count):
vrep.simxGetObjectFloatParameter(self.client_id,
self.handles[handle_idx], 2012,
vrep.simx_opmode_streaming)
for handle_idx in range(0, self.joint_count):
vrep.simxGetJointPosition(self.client_id, self.handles[handle_idx],
vrep.simx_opmode_streaming)
def on_release(key):
vrep.simxSetJointTargetVelocity(clientID, rightMotorHandle, v_Right,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(clientID, leftMotorHandle, v_Left,
vrep.simx_opmode_streaming)
thetaDir = vrep.simxGetJointPosition(clientID, rightMotorHandle,
vrep.simx_opmode_streaming)[1]
thetaEsq = vrep.simxGetJointPosition(clientID, leftMotorHandle,
vrep.simx_opmode_streaming)[1]
localizacao.setAngulos(thetaDir, thetaEsq)
def get_joint_position(self, object_handle):
""" get joint position of an object """
try:
err_code, angle = vrep.simxGetJointPosition(
self.clientID, object_handle, vrep.simx_opmode_streaming)
while err_code != vrep.simx_return_ok:
err_code, angle = vrep.simxGetJointPosition(
self.clientID, object_handle, vrep.simx_opmode_buffer)
return angle
except Exception as e:
raise e
def getCurrentBaseJointPos(self):
res, jointHandle = vrep.simxGetObjectHandle(
self._clientID, GB_CBASE_JOINT_NAME, vrep.simx_opmode_oneshot_wait)
res, pos = vrep.simxGetJointPosition(self._clientID, jointHandle,
vrep.simx_opmode_streaming)
# Wait until the first data has arrived (just any blocking funtion):
vrep.simxGetPingTime(self._clientID)
# Now you can read the data that is being continuously streamed:
res, pos = vrep.simxGetJointPosition(self._clientID, jointHandle,
vrep.simx_opmode_buffer)
return pos
def getJointPositionNonBlock(self, clientID, joint, firstTime):
error = False
value = 0
handle = self.jointHandleMapping[joint]
if (handle == 0):
error, handle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot)
self.jointHandleMapping[joint]=handle
if not error:
if firstTime:
error, value = vrep.simxGetJointPosition(clientID,handle,vrep.simx_opmode_streaming)
else:
error, value = vrep.simxGetJointPosition(clientID,handle,vrep.simx_opmode_buffer)
return error, value
def run360():
print "connecting to vrep with address", IPADDRESS, "on port", PORT
clientID = vrep.simxStart(IPADDRESS, PORT, True, True, 5000, 5)
print "hello vrep! ", clientID
if clientID == -1:
print "failed to connect to vrep server"
return
# get the motor joint handle
error,jointHandle = vrep.simxGetObjectHandle(clientID, "plantStandMotor", vrep.simx_opmode_oneshot_wait)
print "starting the motor..."
# set the velocity of the joint
vrep.simxSetJointTargetVelocity(clientID, jointHandle, 1.0, vrep.simx_opmode_oneshot_wait);
print "spinning 360 degrees..."
# set up to stream joint positions
vrep.simxGetJointPosition(clientID, jointHandle, vrep.simx_opmode_streaming);
passed90Degrees = False
# The control loop:
while vrep.simxGetConnectionId(clientID) != -1 : # while we are connected to the server..
(error,position) = vrep.simxGetJointPosition(
clientID,
jointHandle,
vrep.simx_opmode_buffer
)
# Fetch the newest joint value from the inbox (func. returns immediately (non-blocking)):
if error==vrep.simx_error_noerror :
# here we have the newest joint position in variable jointPosition!
# break when we've done a 360
if passed90Degrees and position >= 0 and position < math.pi/2.0:
break
elif not passed90Degrees and position > math.pi/2.0:
passed90Degrees = True
print "stoppping..."
vrep.simxSetJointTargetVelocity(clientID, jointHandle, 0.0, vrep.simx_opmode_oneshot_wait);
if clientID != -1:
vrep.simxFinish(clientID)
print "done!"
def read_FS():
global gripper_handles
penalty = 0
returnCode, state, forceVector0, torqueVector0 = vrep.simxReadForceSensor(
clientID, FS_handles[0], vrep.simx_opmode_buffer)
returnCode, state, forceVector1, torqueVector1 = vrep.simxReadForceSensor(
clientID, FS_handles[1], vrep.simx_opmode_buffer)
returnCode, state, forceVector2, torqueVector2 = vrep.simxReadForceSensor(
clientID, FS_handles[2], vrep.simx_opmode_buffer)
# print forceVector0, torqueVector0
# print forceVector1, torqueVector1
# print forceVector2, torqueVector2
force0 = (forceVector0[0]**2 + forceVector0[1]**2 +
forceVector0[2]**2)**(0.5)
force1 = (forceVector1[0]**2 + forceVector1[1]**2 +
forceVector1[2]**2)**(0.5)
force2 = (forceVector2[0]**2 + forceVector2[1]**2 +
forceVector2[2]**2)**(0.5)
# print force0, force1, force2
av_force = np.mean([force0, force1, force2])
force_dist = (abs(force0 - force1) + abs(force1 - force2) +
abs(force2 - force0)) / 3
force_dist_cost = 3 * (1 - np.exp(force_dist - 3))
if force_dist_cost < -20:
force_dist_cost = -20
print(av_force, force_dist, force_dist_cost)
returnCode, hand_joint_0 = vrep.simxGetJointPosition(
clientID, gripper_handles[2], vrep.simx_opmode_buffer)
returnCode, hand_joint_1 = vrep.simxGetJointPosition(
clientID, gripper_handles[3], vrep.simx_opmode_buffer)
returnCode, hand_joint_2 = vrep.simxGetJointPosition(
clientID, gripper_handles[4], vrep.simx_opmode_buffer)
if hand_joint_0 > 100 * math.pi / 180 or hand_joint_0 < 40 * math.pi / 180:
penalty = penalty + 10
if hand_joint_1 > 100 * math.pi / 180 or hand_joint_1 < 40 * math.pi / 180:
penalty = penalty + 10
if hand_joint_2 > 100 * math.pi / 180 or hand_joint_2 < 40 * math.pi / 180:
penalty = penalty + 10
return [av_force, force_dist_cost, penalty]
def getJointValues(self): #Store values of waiter
error, rLeg = vrep.simxGetJointPosition(clientID, self.rightLegID,
vrep.simx_opmode_streaming)
error, lLeg = vrep.simxGetJointPosition(clientID, self.leftLegID,
vrep.simx_opmode_streaming)
error, rKnee = vrep.simxGetJointPosition(clientID, self.rightKneeID,
vrep.simx_opmode_streaming)
error, lKnee = vrep.simxGetJointPosition(clientID, self.leftKneeID,
vrep.simx_opmode_streaming)
error, rAnkle = vrep.simxGetJointPosition(clientID, self.rightAnkleID,
vrep.simx_opmode_streaming)
error, lAnkle = vrep.simxGetJointPosition(clientID, self.leftAnkleID,
vrep.simx_opmode_streaming)
error, rElbow = vrep.simxGetJointPosition(clientID, self.rightElbowID,
vrep.simx_opmode_streaming)
error, lElbow = vrep.simxGetJointPosition(clientID, self.leftElbowID,
vrep.simx_opmode_streaming)
error, rShoulder = vrep.simxGetJointMatrix(clientID, self.rightArmID,
vrep.simx_opmode_streaming)
error, lShoulder = vrep.simxGetJointMatrix(clientID, self.leftArmID,
vrep.simx_opmode_streaming)
error, neck = vrep.simxGetJointMatrix(clientID, self.neckID,
vrep.simx_opmode_buffer)
return (rLeg, lLeg, rKnee, lKnee, rAnkle, lAnkle, rElbow, lElbow,
rShoulder, lShoulder, neck)
def get_angles_firsttime():
for i in range(12):
print('handler is')
print(int(angles_handler[i]))
trash, ang = vrep.simxGetJointPosition(clientID,
int(angles_handler[i]),
vrep.simx_opmode_streaming)
res = vrep.simx_return_novalue_flag
while res != vrep.simx_return_ok:
res, ang1 = vrep.simxGetJointPosition(clientID,
int(angles_handler[i]),
vrep.simx_opmode_buffer)
print("Angle x IS")
print(ang1)
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 printJointPositions(self, clientID):
error, handlers, intData, floatData, stringData=vrep.simxGetObjectGroupData(clientID,vrep.sim_appobj_object_type,0,vrep.simx_opmode_oneshot_wait)
itemHandle=0
print stringData
for name in stringData:
error, position = vrep.simxGetJointPosition(clientID,handlers[itemHandle], vrep.simx_opmode_streaming)
itemHandle=itemHandle+1
print name + ":" + str(position)
def handle_output(self):
err, arm_ori = vrep.simxGetJointPosition(self.cid, self.arm_joint,
vrep.simx_opmode_oneshot)
#2012 is the code for joint velocity
err, arm_vel = vrep.simxGetObjectFloatParameter(self.cid,
self.arm_joint, 2012,
vrep.simx_opmode_oneshot)
return [arm_ori, arm_vel]
def get_joint_angles(self, initial=False):
if initial:
mode = vrep.simx_opmode_streaming
else:
mode = vrep.simx_opmode_buffer
angles = []
for joint in self.joints:
rc, angle = vrep.simxGetJointPosition(self.client_id, joint, mode)
assert rc == (1 if initial else 0), rc
angles.append(angle)
return np.array(angles)
# Here, we use maths.radians to convert degrees into radians.
# http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxSetJointTargetPosition
msg = "Motors target positions: " + str(ashoulder) + " " + str(aelbow) + " " + str(awrist)
print msg
logging.info(msg)
vrep.simxSetJointTargetPosition(clientID, wristHandle, math.radians(awrist), opmode)
vrep.simxSetJointTargetPosition(clientID, elbowHandle, math.radians(aelbow), opmode)
vrep.simxSetJointTargetPosition(clientID, shoulderHandle, math.radians(ashoulder), opmode)
# Wait in order to let the motors finish their movements
# Tip: there must be a more efficient way to do it...
time.sleep(3)
# Get the motors effective positions after the movement sequence
# http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxGetJointPosition
pwrist = vrep.simxGetJointPosition(clientID, wristHandle, opmode)
pelbow = vrep.simxGetJointPosition(clientID, elbowHandle, opmode)
pshoulder = vrep.simxGetJointPosition(clientID, shoulderHandle, opmode)
msg = "Motors reached positions: " + str(pshoulder) + " " + str(pelbow) + " " + str(pwrist)
#print msg
# Get the robot position after the movement sequence
pret, lPos = vrep.simxGetObjectPosition(clientID, leftWheelHandle, -1, vrep.simx_opmode_streaming)
pret, rPos = vrep.simxGetObjectPosition(clientID, rightWheelHandle, -1, vrep.simx_opmode_streaming)
msg = "2w1a position after move: leftWheel = " + str(lPos) +\
", rightWheel = " + str(rPos) + ")"
print msg
logging.info(msg)
# find out the fitness robot moved
rs = fitness(lOriginPos, rOriginPos, lPos, rPos)
vrep.simxStartSimulation(clientID, opmode)
theTime = time.time()
# Start getting the robot position
pret, robotPos = vrep.simxGetObjectPosition(clientID, robotHandle, -1, vrep.simx_opmode_streaming)
pret4, leftWheelPos = vrep.simxGetObjectPosition(clientID, leftWheelHandle, -1, vrep.simx_opmode_streaming)
pret5, rightWheelPos = vrep.simxGetObjectPosition(clientID, rightWheelHandle, -1, vrep.simx_opmode_streaming)
print "2w1a position: (x = " + str(robotPos[0]) + ", y = " + str(robotPos[1]) + ")"
# Start getting the robot orientation
oret, robotOrient = vrep.simxGetObjectOrientation(clientID, robotHandle, -1, vrep.simx_opmode_streaming)
print "2w1a orientation: (x = " + str(robotOrient[0]) + \
", y = " + str(robotOrient[1]) +\
", z = " + str(robotOrient[2]) + ")"
for gene in individual.genes:
vrep.simxSetJointTargetPosition(clientID, gene.type, math.radians(gene.action), opmode)
pgene = vrep.simxGetJointPosition(clientID, gene.type, opmode)
#time.sleep(0.05)
# Wait in order to let the motors finish their movements
# timer = time.time()
# while True:
# pgene = vrep.simxGetJointPosition(clientID, gene.type, opmode)
# if round(math.degrees(pgene[1]), 0) == round(gene.action, 0):
# break
# else:
# time.sleep(0.01)
# timer += 0.01
# if time.time() >= timer + 3:
# theTime = 0
initialCall=True
print ('VREP Simulation Program')
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')
else:
print('Connection Failure')
sys.exit('Abort Connection')
# Object handle
_,quadBase=vrep.simxGetObjectHandle(clientID,'Quadricopter_base',vrep.simx_opmode_oneshot_wait)
_,jointPole=vrep.simxGetObjectHandle(clientID,'Pole_joint',vrep.simx_opmode_oneshot_wait)
while True:
# Code for testing...
if initialCall:
mode = vrep.simx_opmode_streaming
initialCall = False
else:
mode = vrep.simx_opmode_buffer
errorFlag,jointPos=vrep.simxGetJointPosition(clientID,jointPole,mode)
if errorFlag == vrep.simx_return_ok:
print(str(jointPos))
else:
print('Measurements Awaiting')
time.sleep(1.0)
target_index += 1
# get the (x,y,z) position of the target
_, target_xyz = vrep.simxGetObjectPosition(clientID,
target_handle,
-1, # retrieve absolute, not relative, position
vrep.simx_opmode_blocking)
if _ !=0 : raise Exception()
track_target.append(np.copy(target_xyz)) # store for plotting
target_xyz = np.asarray(target_xyz)
for ii,joint_handle in enumerate(joint_handles):
old_q = np.copy(q)
# get the joint angles
_, q[ii] = vrep.simxGetJointPosition(clientID,
joint_handle,
vrep.simx_opmode_blocking)
if _ !=0 : raise Exception()
# get the joint velocity
_, dq[ii] = vrep.simxGetObjectFloatParameter(clientID,
joint_handle,
2012, # parameter ID for angular velocity of the joint
vrep.simx_opmode_blocking)
if _ !=0 : raise Exception()
# update end-effector position
L = np.array([0, .35]) # segment lengths associated with each joint
s = np.sin(q)
c = np.cos(q)
def get_joint_values(clientID,Body):
i = 0
angles = [0] * 25
angles[0] = vrep.simxGetJointPosition(clientID,Body[0][i],vrep.simx_opmode_oneshot_wait)[1]
angles[1] = vrep.simxGetJointPosition(clientID,Body[1][i], vrep.simx_opmode_oneshot_wait)[1]
#Left Leg
angles[8] = vrep.simxGetJointPosition(clientID,Body[2][i],vrep.simx_opmode_oneshot_wait)[1]
angles[9] = vrep.simxGetJointPosition(clientID,Body[3][i],vrep.simx_opmode_oneshot_wait)[1]
angles[10] = vrep.simxGetJointPosition(clientID,Body[4][i],vrep.simx_opmode_oneshot_wait)[1]
angles[11] = vrep.simxGetJointPosition(clientID,Body[5][i],vrep.simx_opmode_oneshot_wait)[1]
angles[12] = vrep.simxGetJointPosition(clientID,Body[6][i],vrep.simx_opmode_oneshot_wait)[1]
angles[13] = vrep.simxGetJointPosition(clientID,Body[7][i],vrep.simx_opmode_oneshot_wait)[1]
#Right Leg
angles[14] = vrep.simxGetJointPosition(clientID,Body[8][i],vrep.simx_opmode_oneshot_wait)[1]
angles[15] = vrep.simxGetJointPosition(clientID,Body[9][i],vrep.simx_opmode_oneshot_wait)[1]
angles[16] = vrep.simxGetJointPosition(clientID,Body[10][i],vrep.simx_opmode_oneshot_wait)[1]
angles[17] = vrep.simxGetJointPosition(clientID,Body[11][i],vrep.simx_opmode_oneshot_wait)[1]
angles[18] =vrep.simxGetJointPosition(clientID,Body[12][i],vrep.simx_opmode_oneshot_wait)[1]
angles[19] =vrep.simxGetJointPosition(clientID,Body[13][i],vrep.simx_opmode_oneshot_wait)[1]
#Left Arm
angles[2] = vrep.simxGetJointPosition(clientID,Body[14][i],vrep.simx_opmode_oneshot_wait)[1]
angles[3] =vrep.simxGetJointPosition(clientID,Body[15][i],vrep.simx_opmode_oneshot_wait)[1]
angles[4] = vrep.simxGetJointPosition(clientID,Body[16][i],vrep.simx_opmode_oneshot_wait)[1]
angles[5] = vrep.simxGetJointPosition(clientID,Body[17][i],vrep.simx_opmode_oneshot_wait)[1]
angles[6] =vrep.simxGetJointPosition(clientID,Body[18][i],vrep.simx_opmode_oneshot_wait)[1]
#Right Arm
angles[20] = vrep.simxGetJointPosition(clientID,Body[19][i],vrep.simx_opmode_oneshot_wait)[1]
angles[21] = vrep.simxGetJointPosition(clientID,Body[20][i],vrep.simx_opmode_oneshot_wait)[1]
angles[22] = vrep.simxGetJointPosition(clientID,Body[21][i],vrep.simx_opmode_oneshot_wait)[1]
angles[23] = vrep.simxGetJointPosition(clientID,Body[22][i],vrep.simx_opmode_oneshot_wait)[1]
angles[24] = vrep.simxGetJointPosition(clientID,Body[23][i],vrep.simx_opmode_oneshot_wait)[1]
#angles[7] = 1.0 - vrep.simxGetJointPosition(clientID,Body[25][i][0],vrep.simx_opmode_oneshot_wait)[1]
#angles[25] = 1.0 - vrep.simxGetJointPosition(clientID,Body[25][i][0],vrep.simx_opmode_oneshot_wait)[1]
return angles
e = vrep.simxSetJointTargetPosition(client_id, joint_1, 0, vrep.simx_opmode_streaming)
# this line is necessary to enable position recording
e, body_pos = vrep.simxGetObjectPosition(client_id, body, -1, vrep.simx_opmode_streaming)
# Wait until the robot is settled to the default position
time.sleep(0.3)
# enable synchronous mode
vrep.simxSynchronous(client_id, True)
position_history = []
e, body_pos = vrep.simxGetObjectPosition(client_id, body, -1, vrep.simx_opmode_buffer)
position_history.append(body_pos)
joint_pos_history = []
e, joint_0_pos = vrep.simxGetJointPosition(client_id, joint_0, vrep.simx_opmode_streaming)
e, joint_1_pos = vrep.simxGetJointPosition(client_id, joint_1, vrep.simx_opmode_streaming)
joint_pos_history.append([joint_0_pos, joint_1_pos])
with contexttimer.Timer() as timer:
for i in range(4):
e = vrep.simxSetJointTargetPosition(client_id, joint_0, 0.5, vrep.simx_opmode_streaming)
for t in range(3):
vrep.simxSynchronousTrigger(client_id)
e, body_pos = vrep.simxGetObjectPosition(client_id, body, -1, vrep.simx_opmode_buffer)
position_history.append(body_pos)
e, joint_0_pos = vrep.simxGetJointPosition(client_id, joint_0, vrep.simx_opmode_buffer)
e, joint_1_pos = vrep.simxGetJointPosition(client_id, joint_1, vrep.simx_opmode_buffer)
joint_pos_history.append([joint_0_pos, joint_1_pos])
e = vrep.simxSetJointTargetPosition(client_id, joint_1, 2.5, vrep.simx_opmode_streaming)
def getJointObjectPosition(self, obj):
return vrep.simxGetJointPosition(self.clientID, obj, self.opmode)
rand_hand_bend = numpy.random.uniform(10,90) *math.pi/180
# instruct the arm to move (all joints at once)
vrep.simxPauseCommunication(clientID,True)
vrep.simxSetJointPosition(clientID,armJoint0Handle, rand_base_rotate,vrep.simx_opmode_streaming) # base - rotates the entire arm - [-40..40]
vrep.simxSetJointPosition(clientID,armJoint1Handle, rand_base_lower ,vrep.simx_opmode_streaming) # base - lowers the arm - [5..35]
vrep.simxSetJointPosition(clientID,armJoint2Handle, rand_elbow_bend ,vrep.simx_opmode_streaming) # elbow - bends the arm - [0..50]
vrep.simxSetJointPosition(clientID,armJoint3Handle, rand_hand_bend ,vrep.simx_opmode_streaming) # hand - bends the lower part of the arm - [0..90]
vrep.simxSetJointPosition(clientID,armJoint4Handle, 0.0,vrep.simx_opmode_streaming) # wrist - turns the gripper
vrep.simxPauseCommunication(clientID,False)
# give the arm some time to move
time.sleep(1)
# verify that the arm has approximately reached its target position
err0 = vrep.simxGetJointPosition(clientID,armJoint0Handle,vrep.simx_opmode_streaming)[1] - rand_base_rotate
err1 = vrep.simxGetJointPosition(clientID,armJoint1Handle,vrep.simx_opmode_streaming)[1] - rand_base_lower
err2 = vrep.simxGetJointPosition(clientID,armJoint2Handle,vrep.simx_opmode_streaming)[1] - rand_elbow_bend
err3 = vrep.simxGetJointPosition(clientID,armJoint3Handle,vrep.simx_opmode_streaming)[1] - rand_hand_bend
err4 = vrep.simxGetJointPosition(clientID,armJoint4Handle,vrep.simx_opmode_streaming)[1] - 0.0
joint_pos_err = abs(err0) + abs(err1) + abs(err2) + abs(err3) + abs(err4)
print "joint positioning error =", joint_pos_err
# get image from vision sensor 1
err,res,img = vrep.simxGetVisionSensorImage(clientID,visionSensor1Handle,0,vrep.simx_opmode_oneshot_wait)
colval1 = img_to_numpy(res,img)
# get the binary image marking the cyan ball in Youbot's gripper
binval1 = img_binarise(colval1,0.0,0.15,0.7,0.99,0.9,1.0)
# get the x- and y-coordinates of the cyan ball
binval1 = numpy.reshape(binval1,(res[1],res[0]))
pos_img1 = img_find_cm(binval1)
errorCode,joint_Handle11=vrep.simxGetObjectHandle(clientID,"LBR4p_joint1",vrep.simx_opmode_oneshot_wait) errorCode,joint_Handle5=vrep.simxGetObjectHandle(clientID,"BarrettHand_jointB_1",vrep.simx_opmode_oneshot_wait) errorCode,joint_Handle6=vrep.simxGetObjectHandle(clientID,"BarrettHand_jointC_1",vrep.simx_opmode_oneshot_wait) errorCode,joint_Handle7=vrep.simxGetObjectHandle(clientID,"BarrettHand_jointB_0",vrep.simx_opmode_oneshot_wait) errorCode,joint_Handle8=vrep.simxGetObjectHandle(clientID,"BarrettHand_jointC_0",vrep.simx_opmode_oneshot_wait) errorCode,joint_Handle9=vrep.simxGetObjectHandle(clientID,"BarrettHand_jointB_2",vrep.simx_opmode_oneshot_wait) errorCode,joint_Handle10=vrep.simxGetObjectHandle(clientID,"BarrettHand_jointC_2",vrep.simx_opmode_oneshot_wait) errorCode=vrep.simxSetJointTargetPosition(clientID,joint_Handle1,math.radians(65),vrep.simx_opmode_oneshot_wait) errorCode=vrep.simxSetJointTargetPosition(clientID,joint_Handle2,math.radians(170),vrep.simx_opmode_oneshot_wait) errorCode=vrep.simxSetJointTargetPosition(clientID,joint_Handle3,math.radians(90),vrep.simx_opmode_oneshot_wait) errorCode=vrep.simxSetJointTargetPosition(clientID,joint_Handle12,math.radians(90),vrep.simx_opmode_oneshot_wait)#gives position to the joint5 vrep.simxSetIntegerSignal(clientID,"closing_signal",1,vrep.simx_opmode_oneshot_wait)# sends signal to child script errorCode,joint=vrep.simxGetJointPosition(clientID,joint_Handle7,vrep.simx_opmode_oneshot_wait)# get the position of the joint7 time.sleep(5) while joint >= 0.0104653835297: errorCode=vrep.simxSetJointTargetPosition(clientID,joint_Handle1,math.radians(5),vrep.simx_opmode_oneshot_wait) errorCode=vrep.simxSetJointTargetPosition(clientID,joint_Handle11,math.radians(90),vrep.simx_opmode_oneshot_wait) break; time.sleep(2) errorCode=vrep.simxSetJointTargetPosition(clientID,joint_Handle4,math.radians(10),vrep.simx_opmode_oneshot_wait) errorCode=vrep.simxSetJointTargetPosition(clientID,joint_Handle1,math.radians(35),vrep.simx_opmode_oneshot_wait) vrep.simxSetIntegerSignal(clientID,"closing_signal",0,vrep.simx_opmode_oneshot_wait)
def joint_angle(cid, handle):
err, ret = vrep.simxGetJointPosition(cid, handle, vrep_mode)
return [ret]
def _read_sensors(self):
"""
This method is used for read the ePuck's sensors. Don't use directly,
instead use 'step()'
"""
# Read differents sensors
for s in self._sensors_to_read:
if s == 'a':
# Accelerometer sensor in a non filtered way
if self._accelerometer_filtered:
parameters = ('A', 12, '@III')
else:
parameters = ('a', 6, '@HHH')
self._debug('WARNING: Accelerometer not yet implemented!')
elif s == 'n':
# Proximity sensors
res, prox = vrep.simxGetStringSignal(self._clientID, 'EPUCK_proxSens', vrep.simx_opmode_streaming)
if res != vrep.simx_return_ok:
self._debug("WARNING: Proximity sensors readout failed: ", res)
else:
proxVals = vrep.simxUnpackFloats(prox)
# TODO: Find out actual needed scaling factor
proxVals = [int(x * 1000) for x in proxVals]
self._proximity = tuple(proxVals)
elif s == 'm':
# Floor sensors
res, floor1 = vrep.simxGetFloatSignal(self._clientID, 'EPUCK_mylightSens_0', vrep.simx_opmode_streaming)
if res != vrep.simx_return_ok:
self._debug("WARNING: Floor 1 sensor readout failed: ", res)
res, floor2 = vrep.simxGetFloatSignal(self._clientID, 'EPUCK_mylightSens_1', vrep.simx_opmode_streaming)
if res != vrep.simx_return_ok:
self._debug("WARNING: Floor 2 sensor readout failed: ", res)
res, floor3 = vrep.simxGetFloatSignal(self._clientID, 'EPUCK_mylightSens_2', vrep.simx_opmode_streaming)
if res != vrep.simx_return_ok:
self._debug("WARNING: Floor 3 sensor readout failed: ", res)
# Scale returned values to mimic real robot; current factor is just guessed
self._floor_sensors = (floor1*1800, floor2*1800, floor3*1800)
elif s == 'q':
# Motor position sensor
# First: Get the handles of both motor joints
res, leftMotor = vrep.simxGetObjectHandle(self._clientID, 'ePuck_leftJoint', vrep.simx_opmode_oneshot_wait)
if res != vrep.simx_return_ok:
self._debug("WARNING: Unable to get handle of left motor: ", res)
continue
res, rightMotor = vrep.simxGetObjectHandle(self._clientID, 'ePuck_rightJoint', vrep.simx_opmode_oneshot_wait)
if res != vrep.simx_return_ok:
self._debug("WARNING: Unable to get handle of right motor: ", res)
continue
# Second: Get the actual motor position (in radians)
res, leftPos = vrep.simxGetJointPosition(self._clientID, leftMotor, vrep.simx_opmode_oneshot_wait)
if res != vrep.simx_return_ok:
self._debug("WARNING: Readout of left motor failed: ", res)
continue
res, rightPos = vrep.simxGetJointPosition(self._clientID, rightMotor, vrep.simx_opmode_streaming)
if res != vrep.simx_return_ok:
self._debug("WARNING: Readout of left motor failed: ", res)
continue
self._motor_position = (leftPos, rightPos)
elif s == 'o':
# Light sensors
parameters = ('O', 16, '@HHHHHHHH')
self._debug('WARNING: Light sensors not yet implemented!')
elif s == 'u':
# Microphone
parameters = ('u', 6, '@HHH')
self._debug('WARNING: Microphones not yet implemented!')
elif s == 'e':
# Motor Speed
parameters = ('E', 4, '@HH')
self._debug('WARNING: Motor speed not yet implemented!')
elif s == 'i':
# Do nothing for the camera, is an independent process
pass
else:
self._debug('Unknow type of sensor to read')
def transform(pos_wrtworld):
transmation_matrix = np.array([[-1,0,0,2.23],[0,-1,0,-0.775],[0,0,1,0.0771],[0,0,0,1]])
pos_wrt_robot = np.linalg.solve(transmation_matrix,np.array(pos_wrtworld))
return pos_wrt_robot
if __name__ == "__main__":
rospy.init_node('points_talker', anonymous=True)
error_collect = []
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
joints = [1]*3
angles = np.zeros(3)
for i in range(3):
errorCode,joints[i] = vrep.simxGetObjectHandle(clientID,'IRB140_joint'+str(i+1),vrep.simx_opmode_blocking)
angles[i] =(vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1])
epsilon =6e-2
for i in range(3):
vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot)
# Kp = np.abs(error/np.sum(np.abs(error)))
#Kp = 2000
#Kv = 100
# kp = [2000, 2000,2000]
# kv = [100,100,100]
res, floor_handle = vrep.simxGetObjectHandle(clientID, 'floor_handle', vrep.simx_opmode_blocking)
res, s_handle = vrep.simxGetObjectHandle(clientID, 'kinect', vrep.simx_opmode_blocking)
position = vrep.simxGetObjectPosition(clientID, s_handle, vrep.sim_handle_parent, vrep.simx_opmode_blocking)
orientation = vrep.simxGetObjectOrientation(clientID, s_handle, vrep.sim_handle_parent, vrep.simx_opmode_buffer)
print('-'*8+'Calculating Trajectory'+'-'*8)
# print(position)
def callback(data):
print('inside callback')
data = data.data
#print(data)
#a,b,c = calc_parabola_vertex(data[0], data[1], data[2], data[3], data[4], data[5])
a,b = calc_straight_line(data[0], data[1], data[4], data[5])
print([a,b])
z_req_vision= ((a*(x_req))+(b))/2
# z_req_vision= (a*(x_req**2))+(b*x_req)+c
print('z required from vision'+str(z_req_vision))
print('x required from vision'+str(x_req))
print('-'*8+'Checking if ball can be blocked'+'-'*8)
transformation = np.array([[1,0,0,x_req*1000],[0,1,0,0],[0,0,1,z_req_vision*1000],[0,0,0,1]])
desired_angles = robot.inverseKinematics(transformation)
if desired_angles:
print('-'*8+'Moving'+'-'*8)
error = desired_angles-angles
#error = np.array(error)
error_collect.append(error)
#error_collect.append(error.reshape(3,1))
sum = np.sum(np.abs(error))
Kp = [1000*(sum-np.abs(i))/sum for i in error]
Kv = 100
velocity_error = np.array([0 ,0 ,0])
desired_velocity = np.copy(velocity_error)
for i in range(3):
while abs(error[i])>epsilon:
vrep.simxSetJointTargetVelocity(clientID,joints[i],error[i]*9999/abs(error[i]),vrep.simx_opmode_oneshot)
# if error[i]>0.5:
# vrep.simxSetJointForce(clientID,joints[i],abs(Kp*0.5)+(Kv*velocity_error[i]),vrep.simx_opmode_oneshot)
# else:#+(Kv*velocity_error[i])
if abs(error[i])<epsilon:
vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID,joints[i],abs(Kp[i]*(error[i]))+Kv*velocity_error[i],vrep.simx_opmode_oneshot)
error[i] = desired_angles[i] - (vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1])
#print(vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1])
if abs(error[i])<epsilon:
vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot)
res,velocity = vrep.simxGetObjectFloatParameter(clientID,joints[i],2012,vrep.simx_opmode_blocking)
velocity_error[i] = desired_velocity[i] - velocity
#error_collect.append(error)
#print(error)
# while np.sum(np.abs(error))>epsilon:
# for i in range(6):
# if abs(error[i])<epsilon/6.:
# vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot)
# vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot)
# else:
# vrep.simxSetJointTargetVelocity(clientID,joints[i],error[i]*9999/abs(error[i]),vrep.simx_opmode_oneshot)
# vrep.simxSetJointForce(clientID,joints[i],abs(Kp*(error[i])),vrep.simx_opmode_oneshot)
# error[i] = desired_angles[i] - (vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1])
a = []
for i in range(3):
vrep.simxSetJointTargetVelocity(clientID,joints[i],0,vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID,joints[i],9999,vrep.simx_opmode_oneshot)
a.append(vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1])
# print(a)
else:
for i in range(3):
vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot)
print('hi')
sub_MTML.unregister()
