def goto_position(self, pos):
"""Maneuvers the robotic arm to the given position.
"""
time.sleep(self.sleep_sec)
pos_adjusted = list(pos)
pos_adjusted.append(0.0) # Required by V-REP
pos_adjusted.append(0.0) # Required by V-REP
err, retInts, retFloats, retStrings, retBuff = vrep.simxCallScriptFunction(self.clientID,
self.main_object,
vrep.sim_scripttype_childscript,
'computeAnglesFromGripperPositionEx',
[], pos_adjusted, [], bytearray(),
vrep.simx_opmode_blocking)
if err != vrep.simx_return_ok:
raise RuntimeError("goto_position(): Failed to compute angles")
err, retInts, retFloats, retStrings, retBuff = vrep.simxCallScriptFunction(self.clientID,
self.main_object,
vrep.sim_scripttype_childscript,
'moveToPositionEx',
[], retFloats, [], bytearray(),
vrep.simx_opmode_blocking)
if err != vrep.simx_return_ok:
raise RuntimeError("goto_position(): Failed to move the arm")
# Since we have moved the arm, we need to take stock of positions of all the objects
self.__update_all_object_positions()
def step(self, action, z, skel):
# start = time.time()
for i in range(action.shape[0]):
res, outInts, outFloats, outStrings, outBuffers = vrep.simxCallScriptFunction(clientID=self.clientID,
scriptDescription='NAO',
options=vrep.sim_scripttype_childscript,
functionName="step",
inputInts=[],
inputFloats=action[i, :],
inputStrings=[],
inputBuffer=bytearray(),
operationMode=vrep.simx_opmode_oneshot)
# go to next step of simulation in a synchronous way
for j in range(self.frame_skip):
ret = vrep.simxSynchronousTrigger(self.clientID)
res, outInts, outFloats, outStrings, outBuffers = vrep.simxCallScriptFunction(clientID=self.clientID,
scriptDescription='NAO',
options=vrep.sim_scripttype_childscript,
functionName="get_obs",
inputInts=[],
inputFloats=[],
inputStrings=[],
inputBuffer=bytearray(),
operationMode=vrep.simx_opmode_blocking)
# outFloats: NAO state[14 dim joints, 18 dim joint positions], type: list
# shoulder, elbow, wrist position of both arm. 6*3=18 dim + joint 10 = 28
rew = self.calc_reward(skel, outFloats)
# obs = np.array(outFloats)
# state = np.array(outFloats)
# obs = np.concatenate((np.array(outFloats).flatten(), z.flatten()), axis=0)
# state = np.concatenate((np.array(outFloats).flatten(), z.flatten()), axis=0)
obs = z
if self.rModel:
with torch.no_grad():
mu, logvar = self.rModel.encode(torch.from_numpy(np.array(outFloats)[:14]).unsqueeze(0).float())
robot_state = self.rModel.reparameterize(mu, logvar)
robot_state = robot_state.numpy()
else:
robot_state = z
state = robot_state
done = np.array([outInts[0]])
info = list(outBuffers)
return obs, state, rew, done, info
def reset(self, z, skel=None):
res, outInts, outFloats, outStrings, outBuffers = vrep.simxCallScriptFunction(
clientID=self.clientID,
scriptDescription='NAO',
options=vrep.sim_scripttype_childscript,
functionName="reset",
inputInts=[],
inputFloats=[],
inputStrings=[],
inputBuffer=bytearray(),
operationMode=vrep.simx_opmode_blocking)
# go to next step of simulation
for j in range(self.frame_skip):
ret = vrep.simxSynchronousTrigger(self.clientID)
res, outInts, outFloats, outStrings, outBuffers = vrep.simxCallScriptFunction(
clientID=self.clientID,
scriptDescription='NAO',
options=vrep.sim_scripttype_childscript,
functionName="get_obs",
inputInts=[],
inputFloats=[],
inputStrings=[],
inputBuffer=bytearray(),
operationMode=vrep.simx_opmode_blocking)
obs = z
if self.rModel:
with torch.no_grad():
mu, logvar = self.rModel.encode(
torch.from_numpy(
np.array(outFloats)[:14]).unsqueeze(0).float())
robot_state = self.rModel.reparameterize(mu, logvar)
robot_state = robot_state.numpy()
else:
robot_state = z
state = robot_state
# obs = np.array(outFloats)
# state = np.array(outFloats)
# obs = np.concatenate((np.array(outFloats).flatten(), z.flatten()), axis=0)
# state = np.concatenate((np.array(outFloats).flatten(), z.flatten()), axis=0)
done = outInts[0]
info = outBuffers
return obs, state
def ComputePath(clientID, target, goal, targetbody):
inInts = []
inFloats = []
inStrings = []
inBuffer = bytearray()
ret, Quadricopter_target = vrep.simxGetObjectHandle(
clientID, target, vrep.simx_opmode_oneshot_wait)
ret, targetpos = vrep.simxGetObjectPosition(clientID, Quadricopter_target,
-1,
vrep.simx_opmode_oneshot_wait)
#print targetpos
targetpos = (targetpos[0], targetpos[1] + 0.3, targetpos[2])
#print targetpos
ret = setRobotPosition(clientID, Quadricopter_target, targetpos)
inStrings.extend([target, goal, targetbody])
#print(inStrings)
ret, retInts, retfloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
clientID, 'remoteApiCommandServer', vrep.sim_scripttype_childscript,
'init_statespaces', inInts, inFloats, inStrings, inBuffer,
vrep.simx_opmode_oneshot_wait)
inStrings = []
if (len(retInts) < 1):
print('Error in return')
else:
if (retInts[0] == 1):
#print(retfloats)
return True, retfloats
else:
print('Path could not be computed')
return False, []
def randomize_pads(self):
numb = np.random.randint(low=0, high=len(self.texture_list), size=2)
_, _, _, _, _ = vrep.simxCallScriptFunction(self.client_id, \
'remote_api', vrep.sim_scripttype_childscript, 'randomize_pads', \
[], [], [self.obj_mesh_texture_dir + self.texture_list[numb[0]], \
self.obj_mesh_texture_dir + self.texture_list[numb[1]]], \
bytearray(), vrep.simx_opmode_blocking)
def call_lua_function(self, lua_function, ints=[], floats=[], strings=[], bytes=bytearray(), \
opmode=vrep.simx_opmode_blocking):
return_code, out_ints, out_floats, out_strings, out_buffer = vrep.simxCallScriptFunction( \
self.client_id, 'remote_api', vrep.sim_scripttype_customizationscript, lua_function, \
ints, floats, strings, bytes, opmode)
self.check_for_errors(return_code)
return out_ints, out_floats, out_strings, out_buffer
def get_simulation_time(self):
emptyBuff = bytearray()
res, retInts, retFloats, retString, retBuffer = vrep.simxCallScriptFunction(self.client_id, 'goalConfigurations', \
vrep.sim_scripttype_childscript, \
'getSimulationTime', [], [], \
[], emptyBuff, vrep.simx_opmode_blocking)
return retFloats[0]
def get_object_name(self, objectHandle):
emptyBuff = bytearray()
res, retInts, retFloats, objName, retBuffer = vrep.simxCallScriptFunction(
self.client_id, 'goalConfigurations',
vrep.sim_scripttype_childscript, 'getObjectName', [objectHandle],
[], [], emptyBuff, vrep.simx_opmode_blocking)
return objName[0]
def shuffle_obstacles(self):
vrep.unwrap_vrep(
vrep.simxCallScriptFunction(self._clientID, "Robobo_Scene_random", vrep.sim_scripttype_mainscript, "shuffle_obstacles",
[],[],[],bytearray(),vrep.simx_opmode_blocking)
)
return
def checkCollision(self, start, goal):
# m=simGetObjectMatrix(handle,-1)
# mRot=simBuildMatrix({0,0,0},{0,0,rot})
# newM=simMultiplyMatrices(m,mRot)
# simSetObjectMatrix(handle,-1,newM)
# print ("check collision: ", start, goal)
vrep.simxSetObjectPosition(self.clientID, self.robot.target, -1, goal,
vrep.simx_opmode_oneshot_wait)
collision_handles = []
for table in self.tables:
collision_handles.append(table.sim_table)
for chair in table.chairs:
collision_handles.append(chair)
for human in table.humans:
collision_handles.append(human)
returnCode,collision,_,_,_ = vrep.simxCallScriptFunction(self.clientID,"target",vrep.sim_scripttype_childscript,"isInCollision", \
collision_handles,[],[],bytearray(),vrep.simx_opmode_oneshot_wait)
#vrep.simxSetObjectPosition(self.clientID, self.robot.target, -1, start, vrep.simx_opmode_oneshot_wait)
if len(collision) > 0 and collision[0] == 1:
print("inCollision")
return 1
return 0
def retrieve_state(self):
pos = np.zeros(1)
vel = np.zeros(1)
tip = np.zeros(3)
_, pos[0] = vrep.simxGetJointPosition(self.clientID,
self.jointHandles[0],
vrep.simx_opmode_blocking)
_, vel[0] = vrep.simxGetObjectFloatParameter(self.clientID,
self.jointHandles[0],
2012,
vrep.simx_opmode_blocking)
_, tip_list = vrep.simxGetObjectPosition(self.clientID, self.tipHandle,
-1, vrep.simx_opmode_blocking)
tip = np.asarray(tip_list)
_, _1, jac_list, _2, _3 = vrep.simxCallScriptFunction(
self.clientID, 'Jacobian_Script',
vrep.sim_scripttype_childscript, 'jacobian_retriever', [], [], [],
bytearray(), vrep.simx_opmode_blocking)
jac = np.asarray(jac_list)
jac = np.reshape(jac, (6, 1))
jac = jac[0:3, :]
return {1: pos, 2: vel, 3: tip, 5: jac}
def setGoal(cyl_handle,prev_cyl_handle):
global last_visited
returnCode, cyl_pos = vrep.simxGetObjectPosition(clientID,cyl_handle,-1,vrep.simx_opmode_blocking)
returnCode = vrep.simxSetObjectPosition(clientID,goal_dummy_handle,-1,(cyl_pos[0],cyl_pos[1],cyl_pos[2]),vrep.simx_opmode_blocking)
returnCode, rob_pos = vrep.simxGetObjectPosition(clientID,robot_handle,-1,vrep.simx_opmode_buffer)
returnCode = vrep.simxSetObjectPosition(clientID,start_dummy_handle,-1,(rob_pos[0],rob_pos[1],rob_pos[2]),vrep.simx_opmode_blocking)
returnCode, pos = vrep.simxGetObjectPosition(clientID,start_dummy_handle,-1,vrep.simx_opmode_blocking)
returnCode, pos = vrep.simxGetObjectPosition(clientID,goal_dummy_handle,-1,vrep.simx_opmode_blocking)
returnCode, inints,inflts,instrs,inbuffs = vrep.simxCallScriptFunction(clientID,'LuaFunctions',vrep.sim_scripttype_childscript,'remove_cyl',[cyl_handle,-1],[],[],emptyBuff,vrep.simx_opmode_oneshot_wait)
if prev_cyl_handle!=-1:
returnCode,inints,inflts,instrs,inbuffs = vrep.simxCallScriptFunction(clientID,'LuaFunctions',vrep.sim_scripttype_childscript,'add_cyl',[prev_cyl_handle,-1],[],[],emptyBuff,vrep.simx_opmode_oneshot_wait)
last_visited = cyl_handle
return
def _getProximitySensorValues(self):
"""
:return: numpy float array
current values of enabled proximity sensors
This always implies a request to the robot for sensing proximities .
"""
if not self._proximitySensorsEnabled:
logging.error('cannot read proximity sensors if not enabled')
return self._proximitySensorValues
# read distance sensor values from VRep ePuck simulator
res, ret_ints, ret_floats, ret_strings, ret_buffer = vrep.simxCallScriptFunction(self.__clientID, self._name,
vrep.sim_scripttype_childscript,
'getProxSensorsForRemote', [],
[], [], self.__empty_buff,
vrep.simx_opmode_blocking);
if res == vrep.simx_return_ok:
self._proximitySensorValues = np.array(ret_floats)[self._enabledProximitySensors]
logging.debug('dist values: ', self._proximitySensorValues)
else:
logging.warning('Remote function call getProxSensorsForRemote failed: last proximity values used')
# self._proximitySensorValues = np.full((8, 1), self._noDetectionDistance)
return self._proximitySensorValues
def setImageFromFile(self, pathImg):
#
res,outInts,outFloats,outStrings,outBuffer=\
vrep.simxCallScriptFunction(self.clientID,'EBO_Screen', vrep.sim_scripttype_childscript,\
'applyTexture',[],[],[pathImg],self.emptyBuff, vrep.simx_opmode_blocking)
#
pass
def applyAction(self, action, requestRemoteServer=True):
#
if (requestRemoteServer):
if (self._id == RC.CYOUBOT):
remoteObjectName = 'youBot'
else:
remoteObjectName = RC.CSERVER_REMOTE_API_OBJECT_NAME
scale = 1
if (self._id == RC.CKUKA_ARM_BARRETT_HAND):
scale = 1
elif (self._id == RC.CJACO_ARM_HAND):
hand_eigengrasp_amps = action
dofs = gb_hand_eigengrasp_interface.toDOF(hand_eigengrasp_amps)
action = self.accumulateMove(dofs) # dofs -> jointPoses
action = [action[i] * scale for i in range(len(action))]
# -----------------------------------------------------------------------
#
inputInts = []
inputFloats = action
inputStrings = []
inputBuffer = bytearray()
print('ApplyAction:', action)
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(self._clientID, remoteObjectName, \
vrep.sim_scripttype_childscript, \
CSERVER_REMOTE_FUNC_ROBOT_ACT, \
inputInts, inputFloats, inputStrings, inputBuffer, \
vrep.simx_opmode_oneshot_wait)
# ===================================================================================================================================
#
# move simulation ahead one time step
vrep.simxSynchronousTrigger(self._clientID)
return 1
def set_object_position(self, objectHandle, pos):
emptyBuff = bytearray()
res, retInts, retFloats, retString, retBuffer = vrep.simxCallScriptFunction(self.client_id, 'goalConfigurations',\
vrep.sim_scripttype_childscript, \
'setObjectPosition', [objectHandle], \
[pos.x, pos.y, 0.2], [], emptyBuff, \
vrep.simx_opmode_blocking)
def get_cloud(self):
panda_id = self.cid
# Get handle to camera
sim_ret, cam_handle = vrep.simxGetObjectHandle(
panda_id, 'kinect_depth', vrep.simx_opmode_blocking)
# Get camera pose and intrinsics in simulation
emptyBuff = self.dummybyte
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
panda_id, 'kinect', vrep.sim_scripttype_childscript, 'absposition',
[], [], [], emptyBuff, vrep.simx_opmode_blocking)
R = np.asarray(
[[retFloats[0], retFloats[1], retFloats[2], retFloats[3]],
[retFloats[4], retFloats[5], retFloats[6], retFloats[7]],
[retFloats[8], retFloats[9], retFloats[10], retFloats[11]]])
# print('camera pose is: ',R)
result, state, data = vrep.simxReadVisionSensor(
panda_id, cam_handle, vrep.simx_opmode_blocking)
data = data[1]
pcl = []
for i in range(2, len(data), 4):
p = [data[i], data[i + 1], data[i + 2], 1]
pcl.append(np.matmul(R, p))
pcd = remove_clipping(pcl)
np.savetxt('data.pcd', pcd, delimiter=' ')
insertHeader('data.pcd')
return pcd
def call_childscript_function(self, obj_name, func_name, in_tuple):
return self.RAPI_rc(
vrep.simxCallScriptFunction(self.cID, obj_name,
vrep.sim_scripttype_childscript,
func_name, in_tuple[0], in_tuple[1],
in_tuple[2], in_tuple[3],
vrep.simx_opmode_blocking))
def setup_sim_camera(cid):
'''
Fetch pcd data and object matrix from V-RAP and transform to world frame
:return: Raw pcd data
'''
# Get handle to camera
sim_ret, cam_handle = vrep.simxGetObjectHandle(cid, 'kinect_depth',
vrep.simx_opmode_blocking)
emptyBuff = bytearray()
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
cid, 'kinect', vrep.sim_scripttype_childscript, 'absposition', [], [],
[], emptyBuff, vrep.simx_opmode_blocking)
R = np.asarray([[retFloats[0], retFloats[1], retFloats[2], retFloats[3]],
[retFloats[4], retFloats[5], retFloats[6], retFloats[7]],
[retFloats[8], retFloats[9], retFloats[10],
retFloats[11]]])
print('camera pose is: ', R)
result, state, data = vrep.simxReadVisionSensor(cid, cam_handle,
vrep.simx_opmode_blocking)
data = data[1]
pcl = []
for i in range(2, len(data), 4):
p = [data[i], data[i + 1], data[i + 2], 1]
pcl.append(np.matmul(R, p))
return pcl
def path(endpath,port):
vrep.simxFinish(-1)
clientID=vrep.simxStart('127.0.0.1',port,True,True,5000,5)
if clientID!=-1:
emptyBuff = bytearray()
res,retInts,retFloats,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'Dummy',vrep.sim_scripttype_childscript,'threadFunction',[],endpath,[],emptyBuff,vrep.simx_opmode_blocking)
return path
def set_base_speed(self, adv, rot, side):
print("Cliente ID:", self.client_id)
emptyBuff = bytearray()
retCode, oitInts, outFloats, outStrings, outBuffer = vrep.simxCallScriptFunction(
self.client_id, "Viriato", vrep.sim_scripttype_childscript,
"move_base", [], [adv, rot, side], [], "",
vrep.simx_opmode_blocking)
def set_gripper_properties(self,
collidable=False,
measureable=False,
renderable=False,
detectable=False,
cuttable=False,
dynamic=False,
respondable=False,
visible=False):
"""Sets misc. parameters of the gripper model in the sim.
This is used to accomplish things such as: moving the gripper without
colliding with anything, setting it to be visible & being captured
by the cameras, static so fingers don't move, etc ...
"""
props = [
collidable, measureable, renderable, detectable, cuttable, dynamic,
respondable, visible
]
# V-REP encodes these properties as 'not_xxxxx', so we'll just invert
# them here to make calls in the simulator straightforward
props = [not p for p in props]
r = vrep.simxCallScriptFunction(self.clientID,
'remoteApiCommandServer',
vrep.sim_scripttype_childscript,
'setGripperProperties', props, [], [],
bytearray(), vrep.simx_opmode_blocking)
if r[0] != vrep.simx_return_ok:
raise Exception('Error setting gripper properties. Return code ',
r)
def send_to_ros(self, port, path):
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', port, True, True, 5000, 5)
if clientID != -1:
emptyBuff = bytearray()
_, _, retFloats, _, _ = vrep.simxCallScriptFunction(
clientID, self.bot_config["script"],
vrep.sim_scripttype_childscript,
self.bot_config['thread_function'], [], path, [], emptyBuff,
vrep.simx_opmode_oneshot_wait)
x = len(retFloats)
_, _, _, _, _ = vrep.simxCallScriptFunction(
clientID, self.bot_config["script"],
vrep.sim_scripttype_childscript,
self.bot_config['publish_ros'], [x], retFloats, [], emptyBuff,
vrep.simx_opmode_oneshot_wait)
def path_5_sec(clientID, path):
pos_on_path = 1
emptyBuff = bytearray()
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
clientID, 'Dummy', vrep.sim_scripttype_childscript, 'threadFunction',
[], path, [], emptyBuff, vrep.simx_opmode_oneshot_wait)
return (retFloats)
def isFkRunning(goal, othreshold, emptyBuff):
res,retInts,retFloats,retStrings,retBuffer = vrep.simxCallScriptFunction(clientID, "Baxter_rightArm_joint1", vrep.sim_scripttype_childscript, 'pyGetConfig', [], [], [], emptyBuff, vrep.simx_opmode_blocking)
config = retFloats
for i in range(len(config)):
if abs(config[i] - goal[i]) >= othreshold:
return True
return False
def goAndPutDown(startP, regionY, regionZ, box, hand, base, maxVel, maxAccel, maxJerk, ikSteps, emptyBuff, pThreshold, oThreshold):
# create point above final put down position randomly
aboveFinalP1 = createDummy(startP, box)
#randomY = random.uniform(regionY[0], regionY[1])
#randomZ = random.uniform(regionZ[0], regionZ[1])
#res, aboveFinalPPosition = vrep.simxGetObjectPosition(clientID, aboveFinalP, box, vrep.simx_opmode_blocking)
#aboveFinalPPosition = [aboveFinalPPosition[0], randomY, randomZ]
#vrep.simxSetObjectPosition(clientID, aboveFinalP, box, aboveFinalPPosition, vrep.simx_opmode_blocking)
# go to aboveFinalP
#moveToDummy(aboveFinalP, hand, base, maxVel, maxAccel, maxJerk, ikSteps, emptyBuff, pThreshold, oThreshold)
# rotate
angle, configBeforeRotate = goForRandomRotate(aboveFinalP1, hand, base, maxVel, maxAccel, maxJerk, ikSteps, emptyBuff, oThreshold)
aboveFinalP2 = createDummy(hand, base)
# create put down point
putDownP1 = createDummy(aboveFinalP2, -1)
res, putDownPPosition = vrep.simxGetObjectPosition(clientID, putDownP1, -1, vrep.simx_opmode_blocking)
putDownPPosition = [putDownPPosition[0], putDownPPosition[1], tableHeight + gripperLength + 0.01]
vrep.simxSetObjectPosition(clientID, putDownP1, -1, putDownPPosition, vrep.simx_opmode_blocking)
# down to putDownP
putDownP2 = createDummy(putDownP1,base)
moveToDummy(putDownP2, hand, base, maxVel, maxAccel, maxJerk, ikSteps, emptyBuff, pThreshold, oThreshold)
# open
inInts = [motorForce]
inFloats = [motorVelocity1, motorVelocity2]
res,retInts,retFloats,retStrings,retBuffer = vrep.simxCallScriptFunction(clientID, 'Baxter_rightArm_joint1', vrep.sim_scripttype_childscript, 'pyGripper', inInts, inFloats, [], emptyBuff, vrep.simx_opmode_oneshot_wait)
time.sleep(1.5)
# up to aboveFinal
moveToDummy(aboveFinalP2, hand, base, maxVel, maxAccel, maxJerk, ikSteps, emptyBuff, pThreshold, oThreshold)
#return configBeforeRotate
res=vrep.simxRemoveObject(clientID,aboveFinalP1,vrep.simx_opmode_blocking)
res=vrep.simxRemoveObject(clientID,aboveFinalP2,vrep.simx_opmode_blocking)
res=vrep.simxRemoveObject(clientID,putDownP1,vrep.simx_opmode_blocking)
res=vrep.simxRemoveObject(clientID,putDownP2,vrep.simx_opmode_blocking)
def goForRandomRotate(dummy, hand, base, maxVel, maxAccel, maxJerk, ikSteps, emptyBuff, oThreshold):
res,retInts,configBeforeRotate,retStrings,retBuffer = vrep.simxCallScriptFunction(clientID, 'Baxter_rightArm_joint1', vrep.sim_scripttype_childscript, 'pyGetConfig', [], [], [], emptyBuff, vrep.simx_opmode_oneshot_wait)
configAfterRotate = list(configBeforeRotate)
configAfterRotate[6] = random.uniform(-math.pi, math.pi)
moveToConfig(configAfterRotate, maxVel, maxAccel, maxJerk, fkSteps, emptyBuff, oThreshold)
res, handOrientation = vrep.simxGetObjectOrientation(clientID, hand, base, vrep.simx_opmode_blocking)
return handOrientation[2], configBeforeRotate
def setPose(client_id, display_name, transform=None, parent_handle=-1):
"""Set the pose of an object in the simulation
# Arguments
transform_display_name: name string to use for the object in the vrep scene
transform: 3 cartesian (x, y, z) and 4 quaternion (x, y, z, w) elements, same as vrep
parent_handle: -1 is the world frame, any other int should be a vrep object handle
"""
if transform is None:
transform = np.array([0., 0., 0., 0., 0., 0., 1.])
# 2. Now create a dummy object at coordinate 0.1,0.2,0.3 with name 'MyDummyName':
empty_buffer = bytearray()
res, ret_ints, ret_floats, ret_strings, ret_buffer = vrep.simxCallScriptFunction(
client_id, 'remoteApiCommandServer', vrep.sim_scripttype_childscript,
'createDummy_function', [parent_handle], transform, [display_name],
empty_buffer, vrep.simx_opmode_blocking)
if res == vrep.simx_return_ok:
# display the reply from V-REP (in this case, the handle of the created dummy)
print('SetPose object name:', display_name, ' handle: ', ret_ints[0],
' transform: ', transform)
else:
print('setPose remote function call failed.')
print(''.join(traceback.format_stack()))
return -1
return ret_ints[0]
def get_sonar(self):
emptyBuff = bytearray()
retCode, outInts, outFloats, outStrings, outBuffer = vrep.simxCallScriptFunction(self.client_id, "Funciones", vrep.sim_scripttype_childscript, "SensorSonar", [], [], [], emptyBuff, vrep.simx_opmode_blocking)
#res, sonar = vrep.simxGetObjectHandle(self.client_id, "mysonar", vrep.simx_opmode_oneshot_wait)
#retCode, state, point, handle, normal = vrep.simxReadProximitySensor(self.client_id, sonar, vrep.simx_opmode_oneshot_wait)
#print("sonar:", state, point)
return outFloats
def setup_sim_camera(cid):
'''
Fetch pcd data and object matrix from V-RAP and transform to world frame
:return: Raw pcd data
'''
# Get handle to camera
sim_ret, cam_handle = vrep.simxGetObjectHandle(cid, 'kinect_depth', vrep.simx_opmode_blocking)
# Get camera pose and intrinsics in simulation
# a, b, g = angles[0], angles[1], angles[2]
# Rx = np.array([[1.0, 0.0, 0.0],
# [0.0, np.cos(a), -np.sin(a)],
# [0.0, np.sin(a), np.cos(a)]], dtype=np.float32)
# Ry = np.array([[np.cos(b), 0.0, np.sin(b)],
# [0.0, 1.0, 0.0],
# [-np.sin(b), 0.0, np.cos(b)]], dtype=np.float32)
# Rz = np.array([[np.cos(g), -np.sin(g), 0.0],
# [np.sin(g), np.cos(g), 0.0],
# [0.0, 0.0, 1.0]], dtype=np.float32)
# R = np.dot(Rz, np.dot(Ry, Rx))
emptyBuff = bytearray()
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(cid, 'kinect',
vrep.sim_scripttype_childscript,
'absposition', [], [], [], emptyBuff,
vrep.simx_opmode_blocking)
R = np.asarray([[retFloats[0], retFloats[1], retFloats[2], retFloats[3]],
[retFloats[4], retFloats[5], retFloats[6], retFloats[7]],
[retFloats[8], retFloats[9], retFloats[10], retFloats[11]]])
print('camera pose is: ',R)
result, state, data = vrep.simxReadVisionSensor(cid, cam_handle, vrep.simx_opmode_blocking)
data = data[1]
pcl = []
for i in range(2, len(data), 4):
p = [data[i], data[i + 1], data[i + 2], 1]
pcl.append(np.matmul(R, p))
return pcl
def moveOnPath(leftjoint_handle, rightjoint_handle, robot_handle, path_handle, start_dummy_handle, goal_dummy_handle): # Partial paths was not allowed in the path object. Need to take care of that. pos_on_path=0 distance=0 wheel_sep=(0.208)/(2.0) wheel_radius=(0.0701)/(2.0) while pos_on_path < 1: returnCode, path_pos = vrep.simxCallScriptFunction(clientID,LuaFunctions,sim_scripttype_childscript(1),getPathCoordinates,'PathPlanningTask') # task1_cb/scene 1/LuaFunctions distance=math.sqrt(path_pos[1]**2 + path_pos[2]**2) phi=math.atan(path_pos[2]/path_pos[1]) v_des=0.1 w_des=0.8 * phi v_r = v_des + (wheel_sep * w_des) v_l = v_des - (wheel_sep * w_des) w_r = v_r / wheel_radius w_l = v_l / wheel_radius returnCode = vrep.simxSetJointTargetVelocity(clientID, leftjoint_handle, w_l, vrep_simx_opmode_streaming) returnCode = vrep.simxSetJointTargetVelocity(clientID, rightjoint_handle, w_r, vrep_simx_opmode_streaming) if distance < 0.1: pos_on_path += 0.01 returnCode = vrep.simxSetJointTargetVelocity(clientID, leftjoint_handle, 0, vrep_simx_opmode_streaming) returnCode = vrep.simxSetJointTargetVelocity(clientID, rightjoint_handle, 0, vrep_simx_opmode_streaming)
def testFunction(name):
res,retInts,retFloats,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID, name, vrep.sim_scripttype_childscript,
'testFunction', [], [], ['Anand', 'Saha'], bytearray(), vrep.simx_opmode_blocking)
print(res)
print(retInts)
print(retFloats)
print(retStrings)
print(retBuffer)
def moveToPositionEx(name, pos):
time.sleep(0.5)
res,retInts,retFloats,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID, name, vrep.sim_scripttype_childscript,
'moveToPositionEx', [], pos, [], bytearray(), vrep.simx_opmode_blocking)
print(res)
print(retInts)
print(retFloats)
print(retStrings)
print(retBuffer)
return retFloats
def computeAnglesFromGripperPositionEx(name, pos, vertical=0, radial=0):
pos.append(vertical)
pos.append(radial)
res,retInts,retFloats,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID, name, vrep.sim_scripttype_childscript,
'computeAnglesFromGripperPositionEx', [], pos, [], bytearray(), vrep.simx_opmode_blocking)
print(res)
print(retInts)
print(retFloats)
print(retStrings)
print(retBuffer)
return retFloats
def enableGripperEx(name, enable):
time.sleep(0.7)
i_enable = 1
if not enable:
i_enable = -1
res,retInts,retFloats,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID, name, vrep.sim_scripttype_childscript,
'enableGripperEx', [i_enable], [], [], bytearray(), vrep.simx_opmode_blocking)
print(res)
print(retInts)
print(retFloats)
print(retStrings)
print(retBuffer)
return retFloats
def enable_grip(self, enable):
"""Engages/Disengages the gripper.
"""
time.sleep(self.sleep_sec_min)
i_enable = 1
if not enable:
i_enable = -1
err, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(self.clientID,
self.main_object,
vrep.sim_scripttype_childscript,
'enableGripperEx',
[i_enable], [], [], bytearray(),
vrep.simx_opmode_blocking)
if err != vrep.simx_return_ok:
raise RuntimeError("enable_grip(): Failed to enable/disable grip")
# Since we have altered the grip, we need to take stock of positions of all the objects
self.__update_all_object_positions()
self.gripper_enabled = enable
# Start the simulation:
vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait)
# Load a robot instance: res,retInts,retFloats,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'loadRobot',[],[0,0,0,0],['d:/v_rep/qrelease/release/test.ttm'],emptyBuff,vrep.simx_opmode_oneshot_wait)
# robotHandle=retInts[0]
# Retrieve some handles:
res,robotHandle=vrep.simxGetObjectHandle(clientID,'IRB4600#',vrep.simx_opmode_oneshot_wait)
res,target1=vrep.simxGetObjectHandle(clientID,'testPose1#',vrep.simx_opmode_oneshot_wait)
res,target2=vrep.simxGetObjectHandle(clientID,'testPose2#',vrep.simx_opmode_oneshot_wait)
res,target3=vrep.simxGetObjectHandle(clientID,'testPose3#',vrep.simx_opmode_oneshot_wait)
res,target4=vrep.simxGetObjectHandle(clientID,'testPose4#',vrep.simx_opmode_oneshot_wait)
# Retrieve the poses (i.e. transformation matrices, 12 values, last row is implicit) of some dummies in the scene
res,retInts,target1Pose,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'getObjectPose',[target1],[],[],emptyBuff,vrep.simx_opmode_oneshot_wait)
res,retInts,target2Pose,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'getObjectPose',[target2],[],[],emptyBuff,vrep.simx_opmode_oneshot_wait)
res,retInts,target3Pose,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'getObjectPose',[target3],[],[],emptyBuff,vrep.simx_opmode_oneshot_wait)
res,retInts,target4Pose,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'getObjectPose',[target4],[],[],emptyBuff,vrep.simx_opmode_oneshot_wait)
# Get the robot initial state:
res,retInts,robotInitialState,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'getRobotState',[robotHandle],[],[],emptyBuff,vrep.simx_opmode_oneshot_wait)
# Some parameters:
approachVector=[0,0,1] # often a linear approach is required. This should also be part of the calculations when selecting an appropriate state for a given pose
maxConfigsForDesiredPose=10 # we will try to find 10 different states corresponding to the goal pose and order them according to distance from initial state
maxTrialsForConfigSearch=300 # a parameter needed for finding appropriate goal states
searchCount=2 # how many times OMPL will run for a given task
minConfigsForPathPlanningPath=400 # interpolation states for the OMPL path
minConfigsForIkPath=100 # interpolation states for the linear approach path
collisionChecking=1 # whether collision checking is on or off
# -*- encoding: UTF-8 -*-
import vrep
if __name__ == "__main__":
#INIT V-REP IMAGE SENSOR
vrep.simxFinish(-1)
clientID=vrep.simxStart('127.0.0.2',19997,True,True,5000,5)
if clientID!=-1:
print('Connected to remote API server')
#Get and display the pictures from the camera
#Get the handle of the vision sensor
returnCode = 0;
outInts=[];outFloats=[];outStrings=[];outBuffer = '';
inInts=[];inFloats=[];inStrings=[];inBuffer = '';
returnCode,outInts,outFloats,outStrings,outBuffer=vrep.simxCallScriptFunction(clientID,'Semaforo',vrep.sim_scripttype_childscript,'turnGreen',inInts,inFloats,inStrings,inBuffer,vrep.simx_opmode_blocking)
print ('Make sure both are in the same folder as this file,')
print ('or appropriately adjust the file "vrep.py"')
print ('--------------------------------------------------------------')
print ('')
import sys
import ctypes
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')
# 1. First send a command to display a specific message in a dialog box:
emptyBuff = bytearray()
res,retInts,retFloats,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'displayText_function',[],[],['Hello world!'],emptyBuff,vrep.simx_opmode_blocking)
if res==vrep.simx_return_ok:
print ('Return string: ',retStrings[0]) # display the reply from V-REP (in this case, just a string)
else:
print ('Remote function call failed')
# 2. Now create a dummy object at coordinate 0.1,0.2,0.3 with name 'MyDummyName':
res,retInts,retFloats,retStrings,retBuffer=vrep.simxCallScriptFunction(clientID,'remoteApiCommandServer',vrep.sim_scripttype_childscript,'createDummy_function',[],[0.1,0.2,0.3],['MyDummyName'],emptyBuff,vrep.simx_opmode_blocking)
if res==vrep.simx_return_ok:
print ('Dummy handle: ',retInts[0]) # display the reply from V-REP (in this case, the handle of the created dummy)
else:
print ('Remote function call failed')
# 3. Now send a code string to execute some random functions:
code="local octreeHandle=simCreateOctree(0.5,0,1)\n" \
"simInsertVoxelsIntoOctree(octreeHandle,0,{0.1,0.1,0.1},{255,0,255})\n" \
