def pickup(clientID, connector, object_sensor):
result, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor(
clientID, object_sensor, vrep.simx_opmode_buffer)
#print(result, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector)
attached_shape = None
if detectionState == 1:
close_gripper(clientID)
attached_shape = detectedObjectHandle
vrep.simxSetObjectParent(clientID, attached_shape, connector, True,
vrep.simx_opmode_blocking)
return attached_shape
def render_human(self, chair, position, offset):
x, person = vrep.simxLoadModel(self.clientID,
self.models_path + "Sitting Bill.ttm",
0, vrep.simx_opmode_blocking)
x = vrep.simxSetObjectParent(self.clientID, person, chair, True,
vrep.simx_opmode_blocking)
offset_z = vrep.simxGetObjectFloatParameter(
self.clientID, person, vrep.sim_objfloatparam_modelbbox_max_z,
vrep.simx_opmode_blocking)[1]
vrep.simxSetObjectPosition(self.clientID, person, -1,
(position[0] + offset[0], position[1] +
offset[1], position[2] + offset[2]),
vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(self.clientID, person, -1,
(offset[3], offset[4], offset[5]),
vrep.simx_opmode_oneshot)
# vrep.simxSetModelProperty(self.clientID,person,vrep.sim_modelproperty_not_collidable,vrep.simx_opmode_oneshot)
# x, objs = vrep.simxGetObjects(self.clientID, vrep.sim_handle_all, vrep.simx_opmode_oneshot_wait)
# x, joint = vrep.simxGetObjectHandle(self.clientID, 'Bill_leftElbowJoint', vrep.simx_opmode_oneshot_wait)
# x, shoulder = vrep.simxGetObjectHandle(self.clientID, 'Bill_leftShoulderJoint', vrep.simx_opmode_oneshot_wait)
# rotationMatrix = (-1,0,0,0,0,-1,0,0,0,0,-1,0)
# x = vrep.simxSetSphericalJointMatrix(self.clientID, shoulder, rotationMatrix,vrep.simx_opmode_streaming)
#Moving Rotational Joint
# ang = -math.pi/2
# x = vrep.simxSetJointPosition(self.clientID, joint, ang, vrep.simx_opmode_oneshot)
# x, pos = vrep.simxGetJointPosition(self.clientID, joint, vrep.simx_opmode_streaming)
return person
def render_chair(self, table, position, offset):
x, chair = vrep.simxLoadModel(self.clientID,
self.models_path + "dining chair.ttm", 0,
vrep.simx_opmode_blocking)
x = vrep.simxSetObjectParent(self.clientID, chair, table, True,
vrep.simx_opmode_blocking)
offset_z = math.fabs(
vrep.simxGetObjectFloatParameter(
self.clientID, chair, vrep.sim_objfloatparam_modelbbox_min_z,
vrep.simx_opmode_blocking)[1])
vrep.simxSetObjectPosition(
self.clientID, chair, -1,
(position[0] + offset[0], position[1] + offset[1],
position[2] + offset_z + offset[2]), vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(self.clientID, chair, -1,
(offset[3], offset[4], offset[5]),
vrep.simx_opmode_oneshot)
#returnCode, prop = vrep.simxGetModelProperty(self.clientID,chair,vrep.simx_opmode_blocking)
# print (prop, prop + vrep.sim_modelproperty_not_respondable + vrep.sim_modelproperty_not_collidable)
#returnCode2 = vrep.simxSetModelProperty(self.clientID, chair, prop + vrep.sim_modelproperty_not_respondable, vrep.simx_opmode_blocking)
#returnCode3, prop = vrep.simxGetModelProperty(self.clientID,chair,vrep.simx_opmode_blocking)
# print (returnCode, returnCode2, returnCode3, prop)
# vrep.simxSetModelProperty(self.clientID,chair,vrep.sim_modelproperty_not_collidable,vrep.simx_opmode_oneshot)
return chair
def set_parent(self, parent, keep_pos=True):
"""Set object parent."""
if self._handle < 0:
if self._handle == MISSING_HANDLE:
raise RuntimeError("Could not set parent of {}: missing name "
"or handle.".format(self._name))
if self._handle == REMOVED_OBJ_HANDLE:
raise RuntimeError("Could not set parent of {}: object "
"removed.".format(self._name))
client_id = self.client_id
if client_id is None:
raise ConnectionError(
"Could not set parent of {}: not connected to V-REP remote "
"API server.".format(self._name))
parent_handle = to_handle(parent, "parent")
res = vrep.simxSetObjectParent(client_id, self._handle, parent_handle,
keep_pos, vrep.simx_opmode_blocking)
if res != vrep.simx_return_ok:
raise ServerError("Could not set parent of {}.".format(self._name))
if self._parent is not None:
self._parent.unregister_child(self)
self._parent = None
if isinstance(parent, SceneObject):
parent.register_child(self)
self._parent = parent
def fechaGarra(clientID, num):
_, j1 = vrep.simxGetObjectHandle(clientID, 'junta_m1',
vrep.simx_opmode_blocking)
_, j2 = vrep.simxGetObjectHandle(clientID, 'junta_m2',
vrep.simx_opmode_blocking)
vrep.simxPauseCommunication(clientID, True)
vrep.simxSetJointTargetPosition(clientID, j1, 0.028,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, j2, 0.028,
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
_, s = vrep.simxGetObjectHandle(clientID, 'fixaGarra',
vrep.simx_opmode_blocking)
_, cubo = vrep.simxGetObjectHandle(clientID, 'Cubo' + str(num),
vrep.simx_opmode_blocking)
time.sleep(1)
vrep.simxSetObjectParent(clientID, cubo, s, True,
vrep.simx_opmode_blocking)
def abreGarra(clientID, num):
_, mesa = vrep.simxGetObjectHandle(clientID, 'mesa',
vrep.simx_opmode_blocking)
_, cubo = vrep.simxGetObjectHandle(clientID, 'Cubo' + str(num),
vrep.simx_opmode_blocking)
vrep.simxSetObjectParent(clientID, cubo, mesa, True,
vrep.simx_opmode_blocking)
_, j1 = vrep.simxGetObjectHandle(clientID, 'junta_m1',
vrep.simx_opmode_blocking)
_, j2 = vrep.simxGetObjectHandle(clientID, 'junta_m2',
vrep.simx_opmode_blocking)
vrep.simxPauseCommunication(clientID, True)
vrep.simxSetJointTargetPosition(clientID, j1, 0, vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, j2, 0, vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
time.sleep(1)
return 0
# Get a handle for the movable inverse kinematics dummy
errorCode, target_dummy_handle = vrep.simxGetObjectHandle(clientID, "Dummy", vrep.simx_opmode_oneshot_wait)
# Adding robot dummies representing bounding volumes onto joint
robot_joint_bounding_handles = []
for joint in range(7):
# Make a Dummy for each joint
if joint == 5:
errorCode, bounding_handle = vrep.simxCreateDummy(clientID, 0.10, [125,125,125], vrep.simx_opmode_oneshot_wait)
elif joint == 6:
errorCode, bounding_handle = vrep.simxCreateDummy(clientID, 0.08, [125,125,125], vrep.simx_opmode_oneshot_wait)
else:
errorCode, bounding_handle = vrep.simxCreateDummy(clientID, 0.15, [125,125,125], vrep.simx_opmode_oneshot_wait)
robot_joint_bounding_handles.append(bounding_handle)
vrep.simxSetObjectParent(clientID, robot_joint_bounding_handles[joint], joint_handles[joint], False, vrep.simx_opmode_oneshot_wait)
# Kevin
# # print("Building wall!")
# wall_handles = []
# # for z_pos in range(1, 2):
# for z_pos in range(1, 12, 2):
# for x_pos in range(-10, 10, 2):
# # for x_pos in range(1, 2):
# errorCode, bounding_handle = vrep.simxCreateDummy(clientID, 0.10, [200,200,200], vrep.simx_opmode_oneshot_wait)
# wall_handles.append(bounding_handle)
# vrep.simxSetObjectPosition(clientID,bounding_handle,-1,[0.05 * x_pos, 0.3 , 0.05 * z_pos],vrep.simx_opmode_oneshot_wait)
#
# for i in range(len(wall_handles)):
# errorCode, sphere_pos = vrep.simxGetObjectPosition(clientID, wall_handles[i], -1, vrep.simx_opmode_oneshot_wait)
# p_obstacle_list.append(sphere_pos)
import vrep
import math
import numpy as np
R=15
clientID = vrep.simxStart('127.0.0.1',19997,True,True,5000,5)
_, dsethandle = vrep.simxCreateDummy(clientID, 0.5, None, vrep.simx_opmode_blocking)
for x in np.arange(0,R*2,0.5):
_, dhandle = vrep.simxCreateDummy(clientID, 0.3, [255,0,0], vrep.simx_opmode_blocking)
_ = vrep.simxSetObjectParent(clientID,dhandle,dsethandle,True,vrep.simx_opmode_blocking)
_ = vrep.simxSetObjectPosition(clientID, dhandle, -1, [x+25 ,math.sqrt(R**2-(x-R)**2), 0], vrep.simx_opmode_oneshot)
for x in np.arange(2*R,0,-0.5):
_, dhandle = vrep.simxCreateDummy(clientID, 0.3, [255,0,0], vrep.simx_opmode_blocking)
_ = vrep.simxSetObjectParent(clientID,dhandle,dsethandle,True,vrep.simx_opmode_blocking)
_ = vrep.simxSetObjectPosition(clientID, dhandle, -1, [x+25 ,-math.sqrt(R**2-(x-R)**2), 0], vrep.simx_opmode_oneshot)
_, colHandle = vrep.simxGetCollectionHandle(clientID,"Ref",vrep.simx_opmode_blocking)
_, refHandle, intData, doubleData, strData = vrep.simxGetObjectGroupData(clientID,colHandle,3,vrep.simx_opmode_blocking)
refPos = np.reshape(doubleData,(len(refHandle),3))
for index in range(0,len(refHandle)-1):
curPos = refPos[index]
nextPos = refPos[index+1]
relPos = nextPos-curPos
def grip_off():
# err, pos = vrep.simxGetObjectPosition(clientID, cube, table, vrep.simx_opmode_blocking)
vrep.simxSetObjectParent(clientID, cube, -1, 1, vrep.simx_opmode_blocking)
# vrep.simxSetObjectPosition(clientID, cube, table,pos, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(clientID)
def grip_on():
vrep.simxSetObjectParent(clientID, cube, bvp, 1, vrep.simx_opmode_blocking)
# vrep.simxSetObjectPosition(clientID, cube, bvp, [0.0,0.0,0.0], vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(clientID)
vrep.simx_opmode_blocking)
err, m1 = vrep.simxGetObjectHandle(clientID, "marker1",
vrep.simx_opmode_blocking)
err, m2 = vrep.simxGetObjectHandle(clientID, "marker2",
vrep.simx_opmode_blocking)
err, m03 = vrep.simxGetObjectHandle(clientID, "box_center",
vrep.simx_opmode_blocking)
err, m3 = vrep.simxGetObjectHandle(clientID, "marker3",
vrep.simx_opmode_blocking)
err, m000 = vrep.simxGetObjectHandle(clientID, "UR5_joint1",
vrep.simx_opmode_blocking)
err, table = vrep.simxGetObjectHandle(clientID, "diningTable",
vrep.simx_opmode_blocking)
err, cam = vrep.simxGetObjectHandle(clientID, "sensor_1",
vrep.simx_opmode_blocking)
vrep.simxSetObjectParent(clientID, cube, table, 1, vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, cube, m01, [0.0, 0.0, 0.0],
vrep.simx_opmode_blocking)
jjh = []
for i in range(6):
err, jh = vrep.simxGetObjectHandle(clientID, "UR5_joint{}".format(i + 1),
vrep.simx_opmode_blocking)
jjh += [jh]
def get_jj():
jj = []
for i in range(6):
err, j = vrep.simxGetJointPosition(clientID, jjh[i],
vrep.simx_opmode_blocking)
def drop(clientID, attached_shape):
open_gripper(clientID)
if attached_shape != None:
vrep.simxSetObjectParent(clientID, attached_shape, -1, True,
vrep.simx_opmode_blocking)
