def pour_coffee(object_id_1, object_id_2):
vrep.simxSetIntegerSignal(IDList.clientID.mainID, b'armCommand', 4,
vrep.simx_opmode_oneshot_wait)
time.sleep(35)
ObjectStatus.change('cup_type', 1)
time.sleep(5)
ObjectStatus.change('door_type', 0)
def connect(self, server_addr, server_port):
if self.connected:
raise RuntimeError('Client is already connected.')
attempts = 0
max_attempts = 64
while True:
self.cID = vrep.simxStart(connectionAddress=server_addr,
connectionPort=server_port,
waitUntilConnected=True,
doNotReconnectOnceDisconnected=True,
timeOutInMs=1000,
commThreadCycleInMs=0)
attempts += 1
if self.cID != -1:
self.connected = True
break
elif attempts > max_attempts:
raise RuntimeError('Unable to connect to V-REP.')
else:
print('Unable to connect to V-REP at ', server_addr, ':',
server_port, '. Retrying...')
time.sleep(1)
# Setting up debug signal
vrep.simxSetIntegerSignal(self.cID, 'sig_debug', 1337,
vrep.simx_opmode_blocking)
def step_forward_move(self):
result = {}
if self.synchronous:
if self.moving_queue.empty():
result['flag'] = False
vrep.simxSynchronousTrigger(self.clientID)
# take photos if needed
if self.need_take_photo:
image_0 = self.getImage(0)
image_1 = self.getImage(1)
if image_0 is not None and image_1 is not None:
result['photos'] = [image_0, image_1]
self.need_take_photo = False
else:
speed = self.moving_queue.get()
self.move(self.target_orientation, speed)
self.left_time = self.moving_queue.qsize()
# reset cumul if needed
if self.has_reset_cumul:
vrep.simxSetIntegerSignal(self.clientID, 'clear', 1,
vrep.simx_opmode_oneshot)
self.has_reset_cumul = False
# take photos if needed
if self.need_take_photo:
image_0 = self.getImage(0)
image_1 = self.getImage(1)
if image_0 is not None and image_1 is not None:
result['photos'] = [image_0, image_1]
self.need_take_photo = False
result['flag'] = True
return result
else:
assert 0
def run_threaded_candidate(self, finger_angle=0):
"""Launches a threaded scrip in simulator that tests a grasp candidate.
If the initial grasp has all three fingers touching the objects, then
values for the pre- and post-grasp will be returned. Otherwise, a {-1}
will be returned (grasp was attempted, but initial fingers weren't in
contact with the object).
Note that we also set the gripper to be collidable & respondable so it
is able to interact with the object & table.
"""
# The simulator is going to send these signals back to us, so clear
# them to make sure we're not accidentally reading old values
self._clear_signals()
# self.set_gripper_properties(visible=True, dynamic=True,
# collidable=True, respondable=True)
# Launch the grasp process and wait for a return value
vrep.simxSetIntegerSignal(self.clientID, 'run_grasp_attempt',
finger_angle, vrep.simx_opmode_oneshot)
grasp_failed = wait_for_signal(self.clientID, 'py_grasp_done')
print(grasp_failed)
return grasp_failed
def ur5moveto(self, x, y, z):
"""
Move ur5 to location (x,y,z)
"""
vrep.simxSynchronousTrigger(self.clientID) # 让仿真走一步
self.targetQuaternion[0] = 0.707
self.targetQuaternion[1] = 0
self.targetQuaternion[2] = 0.707
self.targetQuaternion[3] = 0 #四元数
self.targetPosition[0] = x
self.targetPosition[1] = y
self.targetPosition[2] = z
vrep.simxPauseCommunication(self.clientID, True) #开启仿真
vrep.simxSetIntegerSignal(self.clientID, 'ICECUBE_0', 21,
vrep.simx_opmode_oneshot)
for i in range(3):
vrep.simxSetFloatSignal(self.clientID, 'ICECUBE_' + str(i + 1),
self.targetPosition[i],
vrep.simx_opmode_oneshot)
for i in range(4):
vrep.simxSetFloatSignal(self.clientID, 'ICECUBE_' + str(i + 4),
self.targetQuaternion[i],
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(self.clientID, False)
vrep.simxSynchronousTrigger(self.clientID) # 进行下一步
vrep.simxGetPingTime(self.clientID) # 使得该仿真步走完
def set_full_state(self, state):
vrep.simxSetIntegerSignal(self.client_id, 'reset-dynamics', 1,
vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
joints_vector = state['joints']
self.leftArm.set_joints_position(self.client_id, joints_vector[0:3])
self.rightArm.set_joints_position(self.client_id, joints_vector[3:6])
self.leftLeg.set_joints_position(self.client_id, joints_vector[6:9])
self.rightLeg.set_joints_position(self.client_id, joints_vector[9:])
for h, s in zip(self.handles, state['objects']):
return_code1 = vrep.simxSetObjectPosition(self.client_id, h, -1,
s[0], self.opmode)
return_code2 = vrep.simxSetObjectOrientation(
self.client_id, h, -1, s[1], self.opmode)
if return_code1 != 0 or return_code2 != 0:
raise RuntimeError(
'Set full state of handle {} failed!'.format(h))
return_code1 = vrep.simxSetIntegerSignal(self.client_id, 'is-fallen',
0, self.opmode)
return_code2 = vrep.simxSetIntegerSignal(self.client_id,
'is-self-collided', 0,
self.opmode)
if return_code1 != 0 or return_code2:
raise RuntimeError('Reset state flags to 0 failed!')
vrep.simxSynchronousTrigger(self.client_id)
def stop_if_needed(self):
try_count = 0
while True:
try_count += 1
vrep.simxSetIntegerSignal(self.clientID, 'dummySignal', 1,
vrep.simx_opmode_blocking)
# returnCode, ping = vrep.simxGetPingTime(self.clientID)
returnCode, serverState = vrep.simxGetInMessageInfo(
self.clientID, vrep.simx_headeroffset_server_state)
# printlog('\nsimxGetInMessageInfo', returnCode)
# print('\nServer state: ', serverState)
# NOTE: if synchronous mode is needed:
# check http://www.forum.coppeliarobotics.com/viewtopic.php?f=5&t=6603&sid=7939343e5e04b699af2d46f8d6efe7ba
stopped = not (serverState & 1)
if stopped:
if try_count == 1:
print("[ROBOTENV] Simulation is already stopped.")
else:
print("[ROBOTENV] Simulation stopped.")
break
else:
if try_count == 1:
print('[ROBOTENV] Stopping simulation...')
returnCode = vrep.simxStopSimulation(
self.clientID, vrep.simx_opmode_blocking)
if returnCode != vrep.simx_return_ok:
print(
"[ROBOTENV] simxStopSimulation failed, error code:",
returnCode)
def setParams(self, params):
for key, value in params.items():
vrep.simxSetFloatSignal(self.clientID, key, value,
vrep.simx_opmode_oneshot)
vrep.simxSetIntegerSignal(self.clientID, 'clear', 1,
vrep.simx_opmode_oneshot)
vrep.simxSetIntegerSignal(self.clientID, 'stop', 1,
vrep.simx_opmode_oneshot)
def operate_gripper(x):
# Set x to 0 to open, 1 to close the gripper
if x == 0:
returnCode_1, signalValue = vrep.simxGetIntegerSignal(clientID, "_gripperClose", vrep.simx_opmode_streaming)
returnCode_2 = vrep.simxSetIntegerSignal(clientID, "_gripperClose", 0, vrep.simx_opmode_oneshot)
returnCode_3 = vrep.simxSetJointTargetVelocity(clientID, phantom_gripper_close_joint, 2, vrep.simx_opmode_oneshot)
elif x == 1:
returnCode_1, signalValue = vrep.simxGetIntegerSignal(clientID, "_gripperClose", vrep.simx_opmode_streaming)
returnCode_2 = vrep.simxSetIntegerSignal(clientID, "_gripperClose", 1, vrep.simx_opmode_oneshot)
returnCode_3 = vrep.simxSetJointTargetVelocity(clientID, phantom_gripper_close_joint, -2, vrep.simx_opmode_oneshot)
def _connect(self):
vrep.simxFinish(-1)
self.clientID = vrep.simxStart('127.0.0.1', self._portnumber, True,
True, 5000, 5)
if self.clientID != -1:
print("Connected to remote API server!")
vrep.simxSetIntegerSignal(self.clientID, 'asdf', 1,
vrep.simx_opmode_oneshot)
else:
sys.exit('Could not connect!')
def open_door(door_id):
if ObjectStatus.find('cup_type') == -1:
print('open_door something wrong!!!')
elif ObjectStatus.find('cup_type') == 0:
vrep.simxSetIntegerSignal(IDList.clientID.mainID, b'armCommand', 2,
vrep.simx_opmode_oneshot_wait)
# 这里可以改进，不用在这里改door_type，用那个isOpen去调用仿真器吧
time.sleep(12)
ObjectStatus.change('door_type', 1)
else:
vrep.simxSetIntegerSignal(IDList.clientID.mainID, b'armCommand', 22,
vrep.simx_opmode_oneshot_wait)
time.sleep(12)
ObjectStatus.change('door_type', 1)
def grasp(object_id):
object_id = int(object_id)
if object_id == IDList.find('cup_1'): # 'cup0_id'
vrep.simxSetIntegerSignal(IDList.clientID.mainID, b'armCommand', 3,
vrep.simx_opmode_oneshot_wait)
# 看这里能不能在其他地方改，动作执行完之后
time.sleep(16)
ObjectStatus.change('rh_have_cup_type', 1)
elif object_id == IDList.find('cup_2'): # 'cup_id'
vrep.simxSetIntegerSignal(IDList.clientID.mainID, b'armCommand', 33,
vrep.simx_opmode_oneshot_wait)
time.sleep(10)
ObjectStatus.change('lh_have_pot_type', 1)
else:
print("grasp something wrong!!")
def is_hit(self):
hit = 0
res, hit = vrep.simxGetIntegerSignal(self.clientID, 'hit',
vrep.simx_opmode_blocking)
if res == vrep.simx_return_ok:
pass
#print ("hit ", hit) # display the reply from V-REP (in this case, the handle of the created dummy)
else:
print('Remote function call failed: is_hit')
if (hit == 1):
vrep.simxSetIntegerSignal(self.clientID, 'hit', 0,
vrep.simx_opmode_blocking)
return hit
def main():
print ('Program started')
emptybuf = bytearray()
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 2000, 5)
if clientID!=-1:
print ('Connected to remote API server')
# Start the simulation:
vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait)
# start init wheels --------------------------------------------------------------------------------------------
wheelJoints = np.empty(4, dtype=np.int)
wheelJoints.fill(-1) # front left, rear left, rear right, front right
res, wheelJoints[0] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_fl', vrep.simx_opmode_oneshot_wait)
res, wheelJoints[1] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_rl', vrep.simx_opmode_oneshot_wait)
res, wheelJoints[2] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_fr', vrep.simx_opmode_oneshot_wait)
res, wheelJoints[3] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_rr', vrep.simx_opmode_oneshot_wait)
# end init wheels ----------------------------------------------------------------------------------------------
# start init camera --------------------------------------------------------------------------------------------
# change the angle of the camera view (default is pi/4)
res = vrep.simxSetFloatSignal(clientID, 'rgbd_sensor_scan_angle', math.pi / 2, vrep.simx_opmode_oneshot_wait)
# turn on camera
res = vrep.simxSetIntegerSignal(clientID, 'handle_rgb_sensor', 2, vrep.simx_opmode_oneshot_wait);
# get camera object-handle
res, youBotCam = vrep.simxGetObjectHandle(clientID, 'rgbSensor', vrep.simx_opmode_oneshot_wait)
# get first image
err, resolution, image = vrep.simxGetVisionSensorImage(clientID, youBotCam, 0, vrep.simx_opmode_streaming)
time.sleep(1)
# end init camera ----------------------------------------------------------------------------------------------
# programmable space -------------------------------------------------------------------------------------------
colorDet.exercise4_action(clientID, youBotCam)
# end of programmable space --------------------------------------------------------------------------------------------
# Stop simulation
vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
# Close connection to V-REP:
vrep.simxFinish(clientID)
else:
print ('Failed connecting to remote API server')
print ('Program ended')
def Toggle_Suction(clientID, state):
"""
This function either turns on the sucker or turns it off, depending
on the specified state.
"""
# Get handles
returnCode, suckerHandle = vrep.simxGetObjectHandle(
clientID, 'BaxterVacuumCup', vrep.simx_opmode_blocking)
if returnCode != vrep.simx_return_ok:
raise Exception(
'Error ' + str(returnCode) +
' : object handle for BaxterVacuumCup did not return successfully.'
)
# end if
if state == "on":
# turn on sucker
print('Turning on vacuum suction cup')
print(' ')
returnCode = vrep.simxSetIntegerSignal(clientID,
'BaxterVacuumCup_active', 1,
vrep.simx_opmode_blocking)
if returnCode != vrep.simx_return_ok:
raise Exception(
'Error ' + str(returnCode) +
' : BaxterVacuumCup did not successfully activate.')
# end if
else:
# turn off sucker
print('Turning off vacuum suction cup')
print(' ')
returnCode = vrep.simxSetIntegerSignal(clientID,
'BaxterVacuumCup_active', 0,
vrep.simx_opmode_blocking)
if returnCode != vrep.simx_return_ok:
raise Exception(
'Error ' + str(returnCode) +
' : BaxterVacuumCup did not successfully activate.')
# end if
# end if
# end def
def Shoot():
vrep.simxSetJointTargetPosition(clientID, joint_three_handle, np.pi / 8,
vrep.simx_opmode_oneshot)
time.sleep(0.2)
vrep.simxSetJointTargetPosition(clientID, joint_four_handle, np.pi / 8,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, joint_three_handle, -np.pi / 8,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, joint_two_handle, -np.pi * 2 / 3,
vrep.simx_opmode_oneshot)
time.sleep(0.14)
vrep.simxSetIntegerSignal(clientID, 'suc_ON', 0, vrep.simx_opmode_oneshot)
#vrep.simxSetJointTargetPosition(clientID, joint_two_handle, -np.pi/2, vrep.simx_opmode_oneshot)
time.sleep(1)
vrep.simxSetJointTargetPosition(clientID, joint_two_handle, 0,
vrep.simx_opmode_blocking)
def waitForRobot(order, patID):
time.sleep(1)
ptime = None
orderID = 16*(order == 'linen') + patID
signalTo = 'order_ToVREP'
signalBack = 'ptime_FromVREP'
# Send order ID to be processed
vrep.simxSetIntegerSignal(clientID,signalTo,orderID,vrep.simx_opmode_oneshot)
# Start the receiving process
err,ptime=vrep.simxGetFloatSignal(clientID,signalBack,vrep.simx_opmode_streaming)
# While we haven't received anything
while not ptime:
err,ptime=vrep.simxGetFloatSignal(clientID,signalBack,vrep.simx_opmode_buffer)
if err != vrep.simx_return_ok:
print('!!!\nAn error occured while receiving data from vrep...\n!!!')
# Clear signal
vrep.simxClearFloatSignal(clientID,signalBack,vrep.simx_opmode_oneshot)
# Return the signal received (processing time)
return ptime
def initializeSensor(clientID):
# start Hokuyo sensor
res = vrep.simxSetIntegerSignal(clientID, 'handle_xy_sensor', 2,
vrep.simx_opmode_oneshot)
# display range sensor beam
vrep.simxSetIntegerSignal(clientID, 'displaylasers', 1,
vrep.simx_opmode_oneshot)
# Get sensor handle:
hokuyo = getSensorHandles(clientID)
# initialize sensor to retrieve data
# to retrieve sensor data, the streaming opmode streaming is at least one time necessary
res, aux, auxD = vrep.simxReadVisionSensor(clientID, hokuyo[0],
vrep.simx_opmode_streaming)
res, aux, auxD = vrep.simxReadVisionSensor(clientID, hokuyo[1],
vrep.simx_opmode_streaming)
return hokuyo
def main(args):
# Close all open connections (just in case)
vrep.simxFinish(-1)
# Connect to V-REP (raise exception on failure)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID == -1:
raise Exception('Failed connecting to remote API server')
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
# Move the joints of the left arm
moveArm("left", clientID)
# Move the joints of the right arm
moveArm("right", clientID)
# Rotate the torso
moveTorso(clientID)
time.sleep(3)
# Test gripper
#moveGripper1(clientID)
#moveGripper2(clientID)
vrep.simxSetIntegerSignal(clientID, "leftGripperClose", 1,
vrep.simx_opmode_oneshot)
vrep.simxSetIntegerSignal(clientID, "rightGripperClose", 1,
vrep.simx_opmode_oneshot)
time.sleep(3)
# Open the hands
vrep.simxSetIntegerSignal(clientID, "leftGripperClose", 0,
vrep.simx_opmode_oneshot)
vrep.simxSetIntegerSignal(clientID, "rightGripperClose", 0,
vrep.simx_opmode_oneshot)
# Let all animations finish
time.sleep(2)
# Stop simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
# Before closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
vrep.simxGetPingTime(clientID)
# Close the connection to V-REP
vrep.simxFinish(clientID)
def sendSignalVREP(self,signalName, signalValue):
if type(signalValue) == str:
eCode = vrep.simxSetStringSignal(self._VREP_clientId, signalName, asByteArray(signalValue), vrep.simx_opmode_oneshot_wait)
elif type(signalValue) == int:
eCode = vrep.simxSetIntegerSignal(self._VREP_clientId, signalName, signalValue, vrep.simx_opmode_oneshot_wait)
elif type(signalValue) == float:
eCode = vrep.simxSetFloatSignal(self._VREP_clientId, signalName, signalValue, vrep.simx_opmode_oneshot_wait)
else:
raise Exception("Trying to send a signal of unknown data type. Only strings, floats and and ints are accepted")
if eCode != 0:
raise Exception("Could not send string signal", signalValue)
vrep.simxSynchronousTrigger(self._VREP_clientId)
def main():
clientID = vrep_utils.start_simulation()
ur3_utils.check_UR3_exists(clientID)
jointHandles = ur3_utils.get_joint_handles(clientID)
print("> V-REP and UR-3 now setup")
vrep.simxSetIntegerSignal(clientID, 'BaxterVacuumCup_active', 0,
vrep.simx_opmode_oneshot)
time.sleep(2)
print("> Set UR-3")
ur3_utils.set_zero_config(clientID, jointHandles)
theta = np.array([0.001, 1.26, 0.2, 0.2, -PI / 2, 0.001])
ur3_utils.set_joint_position(theta, clientID, jointHandles)
time.sleep(3)
# suction cup
vrep.simxSetIntegerSignal(clientID, 'BaxterVacuumCup_active', 1,
vrep.simx_opmode_oneshot)
# move up
theta = np.array([-1.2, -1.00, -0.4, -0.2, PI / 2, 0.001])
ur3_utils.set_joint_position(theta, clientID, jointHandles)
time.sleep(2)
vrep.simxSetIntegerSignal(clientID, 'BaxterVacuumCup_active', 0,
vrep.simx_opmode_oneshot)
time.sleep(2)
print("> Exiting Simulation")
vrep_utils.end_simulation(clientID)
return
def waitForRobot(order, patID):
time.sleep(1)
ptime = None
orderID = 16 * (order == 'linen') + patID
signalTo = 'order_ToVREP'
signalBack = 'ptime_FromVREP'
# Send order ID to be processed
vrep.simxSetIntegerSignal(clientID, signalTo, orderID,
vrep.simx_opmode_oneshot)
# Start the receiving process
err, ptime = vrep.simxGetFloatSignal(clientID, signalBack,
vrep.simx_opmode_streaming)
# While we haven't received anything
while not ptime:
err, ptime = vrep.simxGetFloatSignal(clientID, signalBack,
vrep.simx_opmode_buffer)
if err != vrep.simx_return_ok:
print('!!!\nAn error occured while receiving data from vrep...\n!!!')
# Clear signal
vrep.simxClearFloatSignal(clientID, signalBack, vrep.simx_opmode_oneshot)
# Return the signal received (processing time)
return ptime
def send_obj_sig(obj_type):
if obj_type == 'Cube':
vrep.simxSetIntegerSignal(clientID, 'sig', 4, vrep.simx_opmode_oneshot)
elif obj_type == 'Cyl':
vrep.simxSetIntegerSignal(clientID, 'sig', 1, vrep.simx_opmode_oneshot)
elif obj_type == 'Sphere':
vrep.simxSetIntegerSignal(clientID, 'sig', 4, vrep.simx_opmode_oneshot)
return
def closeHand(clientID):
#close the gripper
res = vrep.simxSetIntegerSignal(clientID, 'gripper_open', 0,
vrep.simx_opmode_oneshot_wait)
time.sleep(2)
def openHand(clientID):
#open the gripper
res = vrep.simxSetIntegerSignal(clientID, 'gripper_open', 1,
vrep.simx_opmode_oneshot_wait)
joint1 = 300
joint2 = 450
joint3 = 130
joint4 = 140
joint5 = 500
timer = 0
while (1):
errorCode, forceState, forceVector, torqueVector = vrep.simxReadForceSensor(
clientID, force_sensor_handle, vrep.simx_opmode_buffer)
timer += 1
if (forceState > 0 and timer % 10000 == 0):
print("force sensor data", errorCode, forceState, forceVector)
#joint2 -= 10
vrep.simxSetIntegerSignal(clientID, "FBvalue", FBvalue,
vrep.simx_opmode_oneshot)
vrep.simxSetIntegerSignal(clientID, "RLvalue", RLvalue,
vrep.simx_opmode_oneshot)
vrep.simxSetIntegerSignal(clientID, "rotvalue", rotvalue,
vrep.simx_opmode_oneshot)
vrep.simxSetIntegerSignal(clientID, "joint1", joint1,
vrep.simx_opmode_oneshot)
vrep.simxSetIntegerSignal(clientID, "joint2", joint2,
vrep.simx_opmode_oneshot)
vrep.simxSetIntegerSignal(clientID, "joint3", joint3,
vrep.simx_opmode_oneshot)
vrep.simxSetIntegerSignal(clientID, "joint4", joint4,
vrep.simx_opmode_oneshot)
vrep.simxSetIntegerSignal(clientID, "joint5", joint5,
vrep.simx_opmode_oneshot)
def open_hand(self, wait_time):
print"open hand"
returnCode = vrep.simxSetIntegerSignal(self.clientID, "hand", (0), vrep.simx_opmode_oneshot_wait)
time.sleep(wait_time)
while np.linalg.norm(d_pos) > threshold:
res, d_pos = vrep.simxGetObjectPosition(
clientID, base, q_target, vrep.simx_opmode_blocking)
res, t_pos = vrep.simxGetObjectPosition(
clientID, q_target, -1, vrep.simx_opmode_blocking)
res, q_pos = vrep.simxGetObjectPosition(
clientID, base, -1, vrep.simx_opmode_blocking)
t_pos[2] = q_pos[2]
vrep.simxSetObjectPosition(clientID, base,-1,t_pos, vrep.simx_opmode_blocking)
print("到达目标点", index, point1)
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
vrep.simxSetIntegerSignal(clientID,'close_hand',0,vrep.simx_opmode_blocking)
res, target = vrep.simxGetObjectHandle(
clientID, "Target", vrep.simx_opmode_blocking)
res, q_target = vrep.simxGetObjectHandle(
clientID, "Quadricopter_target", vrep.simx_opmode_blocking)
res, base = vrep.simxGetObjectHandle(
clientID, "Quadricopter_base", vrep.simx_opmode_blocking)
if clientID != -1:
print('Connected to remote API server')
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
res, target_position = vrep.simxGetObjectPosition(
clientID, target, -1, vrep.simx_opmode_blocking)
path = []
path.append([0,0,-0.35])
path.append([0,0,-0.04])
run(path)
print("init force sensor", errorCode, forceState, forceVector, torqueVector)
#integer signals
Apimode = 1 #work under api mode or UI mode
movementMode = 0 #work under FK(0) or IK(1)
#float signals
Joint = [500, 500, 500, 500, 500, 500] #FK mode joint values
#IK mode position X
pos_X = 0
pos_Y = 0
pos_Z = 0
Alpha = 0
Beta = 0
Gamma = 0
vrep.simxSetIntegerSignal(clientID, "Apimode", Apimode,
vrep.simx_opmode_oneshot)
vrep.simxSetIntegerSignal(clientID, "movementMode", movementMode,
vrep.simx_opmode_oneshot)
if (movementMode):
vrep.simxSetFloatSignal(clientID, "pos_X", pos_X, vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, "pos_Y", pos_Y, vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, "pos_Z", pos_Z, vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, "Alpha", Alpha, vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, "Beta", Beta, vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, "Gamma", Gamma, vrep.simx_opmode_oneshot)
else:
vrep.simxSetFloatSignal(clientID, "Joint1", Joint[0],
vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, "Joint2", Joint[1],
vrep.simx_opmode_oneshot)
vrep.simxSetFloatSignal(clientID, "Joint3", Joint[2],
def integer_signal(cid, handle, val, signal_name):
err = vrep.simxSetIntegerSignal(cid, signal_name, int(val), vrep_mode)
myfct.move_robot(clientID, theta_test, 1, robot0_handles, ref_handles)
#Get object Pose
obj_T, euler = myfct.get_object_T(clientID, obj_handle, world_handle)
robo_Ts = myfct.get_robo_T_from_obj_T(obj_T)
#print(robo_Ts[0])
#theta0 = ece470.findIK(robo_Ts[0],S0,M0) #NEED M's and S's
theta0 = myfct.inverse_kinematic(
robo_Ts[0][0:3, 3], myfct.rotationMatrixToEulerAngles(robo_Ts[0][0:3,
0:3]), 0)
myfct.move_robot(clientID, theta0, 0, robot0_handles, ref_handles)
#Activation:
time.sleep(2)
vrep.simxSetIntegerSignal(clientID, robo_0_cup, 1, vrep.simx_opmode_oneshot)
time.sleep(2)
euler = np.array([3.14, 3.14, 3.14])
euler.shape = (3, 1)
position = np.array([-0.3, 0, 0.6])
position.shape = (3, 1)
print(euler)
theta_start = myfct.inverse_kinematic(position, euler, 0)
#theta_start = np.array([0,0,0,0,0,0]) #np.zeros((6,1))
#theta_start.shape = (6,1)
myfct.move_robot(clientID, theta_start, 0, robot0_handles,
ref_handles) #Back to zero position
#Get object Pose
#obj_T, euler = myfct.get_object_T(clientID,obj_handle, world_handle)
obj_T = np.array([[1, 0, 0, position[0]], [0, 1, 0, position[1]],
def close_gripper(self):
vrep.simxSetIntegerSignal(self.clientID, b'gripperCommand', 0, vrep.simx_opmode_oneshot_wait)
def setEmotionalExpression(self, emotion):
self.__emotion=emotion
vrep.simxSetIntegerSignal(self.__clientID, self.__name+":Emotion", self.__emotion, vrep.simx_opmode_oneshot)
print ('Make sure both are in the same folder as this file,')
print ('or appropriately adjust the file "vrep.py"')
print ('--------------------------------------------------------------')
print ('')
import time
import sys
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')
# setup a useless signal
vrep.simxSetIntegerSignal(
clientID,'asdf',1,vrep.simx_opmode_blocking)
for j in range(5):
print('---------------------simulation',j)
# IMPORTANT
# you should poll the server state to make sure
# the simulation completely stops before starting a new one
while True:
# poll the useless signal (to receive a message from server)
vrep.simxGetIntegerSignal(
clientID,'asdf',vrep.simx_opmode_blocking)
# check server state (within the received message)
e = vrep.simxGetInMessageInfo(clientID,
vrep.simx_headeroffset_server_state)
def integer_signal(cid, handle, val, signal_name):
err = vrep.simxSetIntegerSignal(cid, signal_name, int(val), vrep_mode)
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)
