def getObjPos(self):
e, handle = vrep.simxGetObjectHandle(self.CLIENT_ID,
'MP_BODY_respondable',
vrep.simx_opmode_oneshot_wait)
_, pos = vrep.simxGetObjectPosition(self.CLIENT_ID, handle, -1,
vrep.simx_opmode_oneshot_wait)
return handle, pos
def __init__(self):
self._low_degrees_bound = 0
self._low_radians_bound = 0
self._high_degrees_bound = 180
self._high_radians_bound = np.pi
# Get client id from the V-REP server-side
vrep.simxFinish(-1)
self.conn_handler = vrep.simxStart('127.0.0.1', 19999, True, True,
5000, 5)
if self.conn_handler == -1:
print('Failed connecting to the remote API server')
return -1
print('Succesfully connected to the remote API server...')
# Get UARM entity
# Implemented it in the dict style for future purposes
# (if there're several robots in the env, call them like
# env.robot['uarm'], env.robot['universal_robotics'], etc)
uarm = UARM(self.conn_handler)
self.robot = {'uarm': uarm}
# Get cameras from V-REP
# vs_0 - global view,
# vs_1 - details basket,
# vs_2 - destination basket,
self.camera_handlers = []
for i in range(3):
err, handler = vrep.simxGetObjectHandle(
self.conn_handler, 'vs_' + str(i),
vrep.simx_opmode_oneshot_wait)
if err == vrep.simx_return_ok:
self.camera_handlers.append(handler)
else:
print('Error getting camera handlers, ERR:', err)
return
def get_handles():
global MOTOR_HANDLES
jointNames = {
1: 'j_shoulder_r',
2: 'j_shoulder_l',
3: 'j_high_arm_r',
4: 'j_high_arm_l',
5: 'j_low_arm_r',
6: 'j_low_arm_l',
7: 'j_pelvis_r',
8: 'j_pelvis_l',
9: 'j_thigh1_r',
10: 'j_thigh1_l',
11: 'j_thigh2_r',
12: 'j_thigh2_l',
13: 'j_tibia_r',
14: 'j_tibia_l',
15: 'j_ankle1_r',
16: 'j_ankle1_l',
17: 'j_ankle2_r',
18: 'j_ankle2_l',
19: 'j_pan',
20: 'j_tilt'
}
for i in range(1, 21):
_, handle = vrep.simxGetObjectHandle(CLIENT_ID, jointNames[i],
vrep.simx_opmode_oneshot_wait)
MOTOR_HANDLES[i] = handle
def reset(self):
e, handle = vrep.simxGetObjectHandle(self.CLIENT_ID,
'MP_BODY_respondable',
vrep.simx_opmode_oneshot_wait)
vrep.simxRemoveModel(self.CLIENT_ID, handle, vrep.simx_opmode_blocking)
vrep.simxLoadModel(self.CLIENT_ID, 'darwin.ttm', 0,
vrep.simx_opmode_blocking)
def getEuler(self):
e, handle = vrep.simxGetObjectHandle(self.CLIENT_ID,
'MP_BODY_respondable',
vrep.simx_opmode_oneshot_wait)
print handle
_, euler = vrep.simxGetObjectOrientation(self.CLIENT_ID, handle, -1,
vrep.simx_opmode_oneshot_wait)
print euler
def get_handles(self):
for i in range(1, 21):
e, handle = vrep.simxGetObjectHandle(self.CLIENT_ID,
self.jointNames[i],
vrep.simx_opmode_oneshot_wait)
if e != 0:
print "Error = ", e
exit(1)
self.MOTOR_HANDLES[i] = handle
for i in self.objectnames:
e, handle = vrep.simxGetObjectHandle(self.CLIENT_ID, i,
vrep.simx_opmode_oneshot_wait)
if e != 0:
print "Error = ", e
exit(1)
self.OBJECT_HANDLES.append(handle)
def start_motors(self):
"""
Function to start the motors.
Returns:
A dictionary that contains both motors handle ID.
"""
res, left_handle = vrep.simxGetObjectHandle(self.clientID, "Pioneer_p3dx_leftMotor", vrep.simx_opmode_oneshot_wait)
if(res != vrep.simx_return_ok):
print("\033[93m Left motor not connected.")
else:
print("\033[92m Left motor connected.")
res, right_handle = vrep.simxGetObjectHandle(self.clientID, "Pioneer_p3dx_rightMotor", vrep.simx_opmode_oneshot_wait)
if(res != vrep.simx_return_ok):
print("\033[93m Right motor not connected.")
else:
print("\033[92m Right motor connected.")
return {"left": left_handle, "right":right_handle}
def start_robot(self):
"""
Function to start the robot.
Returns:
robot_handle: Contains the robot handle ID.
"""
res, robot_handle = vrep.simxGetObjectHandle(self.clientID, "Pioneer_p3dx", vrep.simx_opmode_oneshot_wait)
if(res != vrep.simx_return_ok):
print("\033[93m Robot not connected.")
else:
print("\033[92m Robot connected.")
return robot_handle
def start_sensors(self):
"""
Function to start the sensors.
Returns:
us_handle: List that contains each ultrassonic sensor handle ID.
vision_handle: Contains the vision sensor handle ID.
laser_handle: Contains the laser handle ID.
"""
#Starting ultrassonic sensors
us_handle = []
sensor_name=[]
for i in range(0,16):
sensor_name.append("Pioneer_p3dx_ultrasonicSensor" + str(i+1))
res, handle = vrep.simxGetObjectHandle(self.clientID, sensor_name[i], vrep.simx_opmode_oneshot_wait)
if(res != vrep.simx_return_ok):
print ("\033[93m "+ sensor_name[i] + " not connected.")
else:
print ("\033[92m "+ sensor_name[i] + " connected.")
us_handle.append(handle)
#Starting vision sensor
res, vision_handle = vrep.simxGetObjectHandle(self.clientID, "Vision_sensor", vrep.simx_opmode_oneshot_wait)
if(res != vrep.simx_return_ok):
print ("\033[93m Vision sensor not connected.")
else:
print ("\033[92m Vision sensor connected.")
#Starting laser sensor
res, laser_handle = vrep.simxGetObjectHandle(self.clientID, "fastHokuyo", vrep.simx_opmode_oneshot_wait)
if(res != vrep.simx_return_ok):
print ("\033[93m Laser not connected.")
else:
print ("\033[92m Laser connected.")
return us_handle, vision_handle, laser_handle
def get_handles(self):
cam_rgb_names = ["Camera"]
cam_rgb_handles = [
vrep.simxGetObjectHandle(self.client_id, n,
vrep.simx_opmode_blocking)[1]
for n in cam_rgb_names
]
octree_handle = vu.get_handle_by_name(self.client_id, 'Octree')
vision_sensor = vu.get_handle_by_name(self.client_id, 'Vision_sensor')
return dict(
octree=octree_handle,
cam_rgb=cam_rgb_handles,
vision_sensor=vision_sensor,
)
def get_handles(self):
cam_rgb_names = ["Camera"]
cam_rgb_handles = [
vrep.simxGetObjectHandle(self.client_id, n,
vrep.simx_opmode_blocking)[1]
for n in cam_rgb_names
]
octree_handle = vu.get_handle_by_name(self.client_id, 'Octree')
anchor_obj_handle = vu.get_handle_by_name(self.client_id, 'Cuboid0')
other_obj_handle = vu.get_handle_by_name(self.client_id, 'Cuboid')
return dict(
anchor_obj=anchor_obj_handle,
other_obj=other_obj_handle,
octree=octree_handle,
cam_rgb=cam_rgb_handles,
)
def __init__(self, connection_handler):
""" Get all V-REP handlers for the UARM manipulator
Arguments:
connection_handler -- handler containing connection identificator with server-side V-REP
"""
self.conn_handler = connection_handler
self.motors_number = 4
self.motor_handlers = []
self.gripper_handler = None
for i in range(0, self.motors_number):
error, vrep_motor_handler = vrep.simxGetObjectHandle(
self.conn_handler, 'uarm_motor' + str(i + 1),
vrep.simx_opmode_oneshot_wait)
if error == vrep.simx_return_ok:
self.motor_handlers.append(vrep_motor_handler)
else:
print('Error getting motor handlers, ERR: ' + str(error))
return
self.gripper_handler = 'uarm_suctionCup'
def get_handle_by_name(clientID, name):
response, handle = vrep.simxGetObjectHandle(clientID, name,
vrep.simx_opmode_blocking)
assert response == 0, 'Expected return code of 0, but instead return code was {}'.format(
response)
return handle
vrep.simxFinish(-1) # just in case, close all opened connections
client_id = vu.connect_to_vrep()
print(client_id)
res = vu.load_scene(client_id,
'/home/mohit/projects/cooking/code/sim_vrep/scene_0.ttt')
print(res)
if res != vrep.simx_return_ok:
print("Error in loading scene: {}".format(res))
vu.start_sim(client_id)
cam_rgb_names = ["Camera"]
cam_rgb_handles = [
vrep.simxGetObjectHandle(client_id, n, vrep.simx_opmode_blocking)[1]
for n in cam_rgb_names
]
print("Camera handle: {}".format(cam_rgb_handles))
octree_handle = vu.get_handle_by_name(client_id, 'Octree')
print('Octree handle: {}'.format(octree_handle))
voxel_list = []
for t in range(100):
vu.step_sim(client_id)
res = \
vrep.simxCallScriptFunction(
client_id,
"Octree",
vrep.sim_scripttype_childscript,
def setEuler(self):
e, handle = vrep.simxGetObjectHandle(self.CLIENT_ID,
'MP_BODY_respondable',
vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectOrientation(self.CLIENT_ID, handle, -1, [0, 0, 0],
vrep.simx_opmode_oneshot_wait)
def setObjPos(self):
e, handle = vrep.simxGetObjectHandle(self.CLIENT_ID,
'MP_BODY_respondable',
vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectPosition(self.CLIENT_ID, handle, -1, [0.6, 0.6, 0.6],
vrep.simx_opmode_oneshot_wait)
print "Error = ",e
exit(1)
MOTOR_HANDLES[i] = handle
def getPos():
angles = []
for i in MOTOR_HANDLES.keys():
_,angle = vrep.simxGetJointPosition(CLIENT_ID,MOTOR_HANDLES[i],vrep.simx_opmode_oneshot_wait)
angles.append(math.degrees(angle))
return angles
def setPos(writ):
for key,val in writ.items():
vrep.simxSetJointTargetPosition(CLIENT_ID,MOTOR_HANDLES[key],math.radians(val),vrep.simx_opmode_oneshot_wait)
connectVrep()
get_handles()
state = getPos()
print(state)
angle = 5
errCode,motor2 =vrep.simxGetObjectHandle(CLIENT_ID,'j_shoulder_r',vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(CLIENT_ID,motor2,-angle,vrep.simx_opmode_oneshot)
time.sleep(3)
state = getPos()
print(state)