def exists(name: str) -> bool:
"""Checks if the given object is in the scene.
:param id: name/id of object. If the name is appended by a "@alt"
suffix, then the object handle based on the object's alternative
name will be retrieved.
:return: True of the object exists.
"""
try:
vrep.simGetObjectHandle(name)
except:
return False
return True
def __init__(self, name_or_handle: Union[str, int]):
if isinstance(name_or_handle, int):
self._handle = name_or_handle
else:
self._handle = vrep.simGetObjectHandle(name_or_handle)
assert_type = self._get_requested_type()
actual = ObjectType(vrep.simGetObjectType(self._handle))
if actual != assert_type:
raise WrongObjectTypeError(
'You requested object of type %s, but the actual type was '
'%s' % (assert_type.name, actual.name))
def __init__(self, count: int, num_propellers: int,
distance_from_target: float, name: str):
"""Count is used for when we have multiple copies of mobile bases.
:param count: used for multiple copies of robots
:param num_propellers: number of actuated propellers
:param name: string with robot name (same as base in vrep model).
"""
joint_names = [
'%s_propeller_joint%s' % (name, str(i + 1))
for i in range(num_propellers)
]
super().__init__(count, name, joint_names)
self.dist1 = distance_from_target
self.num_propellers = num_propellers
self.static_propeller_velocity = 5.335
self.pParam = 2
self.iParam = 0
self.dParam = 0
self.vParam = -2
self.cumul = 0
self.lastE = 0
self.pAlphaE = 0
self.pBetaE = 0
self.psp2 = 0
self.psp1 = 0
self.prevEuler = 0
suffix = '' if count == 0 else '#%d' % (count - 1)
propeller_names = [
'%s_propeller_respondable%s%s' % (name, str(i + 1), suffix)
for i in range(self.num_propellers)
]
self.propellers = self.get_propeller_handles(
propeller_names) # use handles instead of shapes here
# Motion planning handles
self.intermediate_target_base = Dummy('%s_intermediate_target_base%s' %
(name, suffix))
self.target_base = Shape('%s_target_base%s' % (name, suffix))
self._object_base = vrep.simGetObjectHandle(
'%s_base%s' % (name, suffix)
) # object instead of collection of objects for quadracopter
# Robot parameters and handle
self.z_pos = self.get_position()[2]
self.target_z = self.target_base.get_position()[-1]
def __init__(self, duration=None, mode=None):
self.dur = duration
self.mode = mode
# these names are defined in the VREP scene
self.prox_sensor_prefix = "sensor_model"
self.vis_sensor_prefix = "Vision_sensor#"
self.human = 'Bill'
self.robot = 'UR10'
self.world = 'world_frame'
self.floor = 'sim_floor'
self.pedestal = 'UR10_pedestal'
self.links = ['Base', 'Elbow', 'Tool']
self.robot_links = ['UR10_link2_visible', 'UR10_link3_visible', 'UR10_link4_visible', 'UR10_link7_visible']
self.link_handles = [vrep.simGetObjectHandle(each) for each in self.robot_links]
# self.robot_shield = [self.cylinder(0.15, np.array([0, 0, 0.3])), self.cylinder(0.15, np.array([0, 0, 0.8])),
# self.cylinder(0.15, np.array([0, 0, 0.8])), self.cylinder(0.15, np.array([0, 0, 0.3]))]
self.link_margins = [(pt([0, 0, -0.30]), pt([0, 0, 0.30]), 0.3),
(pt([0, 0, 0]), pt([0, 0, 1.5]), 1),
(pt([0, 0, -0.5]), pt([0, 0, 0.5]), 0.3),
(pt([0, 0, -0.30]), pt([0, 0, 0.30]), 0.3)]
self.link_cylinders = [Cylinder(0.15, 0.5), Cylinder(0.15, 0.8), Cylinder(0.15, 0.8), Sphere(0.3)]
# safety stuff
self.safety_margin = 0.6
# handles
self.human_handle = vrep.simGetObjectHandle(self.human)
self.world_handle = vrep.simGetObjectHandle(self.world)
self.robot_handle = vrep.simGetObjectHandle(self.robot)
self.floor_handle = vrep.simGetObjectHandle(self.floor)
self.pedestal_handle = vrep.simGetObjectHandle(self.pedestal)
self.table_handle = vrep.simGetObjectHandle('customizableTable')
self.table_handle1 = vrep.simGetObjectHandle('customizableTable0')
# handles for self (not referring to the object)
self.robot_handles = vrep.simGetObjectsInTree(self.robot_handle, vrep.sim_handle_all, 0)
self.pedestal_elements = vrep.simGetObjectsInTree(self.pedestal_handle, vrep.sim_handle_all, 0)
self.floor_elements = vrep.simGetObjectsInTree(self.floor_handle, vrep.sim_handle_all, 0)
self.table = vrep.simGetObjectsInTree(self.table_handle, vrep.sim_handle_all, 0)
self.table1 = vrep.simGetObjectsInTree(self.table_handle1, vrep.sim_handle_all, 0)
self.environment = self.robot_handles + self.floor_elements + self.pedestal_elements + self.table + self.table1
# print(len(self.environment))
# handles for non-self
self.non_self = vrep.simGetObjectsInTree(self.human_handle, vrep.sim_handle_all, 0)
print(self.environment, self.non_self)
def get_object_handle(self, object_name):
return vrep.simGetObjectHandle(object_name)
def get_object_type(name: str) -> ObjectType:
"""Gets the type of the object.
:return: Type of the object.
"""
return ObjectType(vrep.simGetObjectType(vrep.simGetObjectHandle(name)))
def main():
data = deque()
ground_truth = deque()
# RTDE Stuff
rtd = rtde_helper('conf/record_configuration.xml', 'localhost', 30004, 125)
# ZMQ stuff
zmq_port = "5556"
context = zmq.Context()
socket = context.socket(zmq.PUSH)
socket.bind("tcp://*:%s" % zmq_port)
# Launch the application with a scene file in headless mode
pr = PyRep()
pr.launch('/home/sarthak/PycharmProjects/research_ws/scenes/bill_new.ttt',
headless=True)
pr.set_simulation_timestep(0.1)
expr = Experiment(5, 0)
ur10 = UR10()
pr.start() # Start the simulation
time.sleep(0.1)
pr.step()
velocities = [5, 5, 5, 5, 5, 5]
ur10.set_joint_target_velocities(velocities)
sim_step = 0
STEPS = 150000
buffer = deque()
depth_cam = vrep.simGetObjectHandle('eyecam') # Bill head cam
# js_thread = Thread(target=sim_work, args=(rtd, pr, ur10))
pr.step()
time.sleep(5)
data_store = []
while True: #sim_step <= STEPS:
q, _ = rtd.get_joint_states()
# ur10.set_joint_target_positions(q)
#q = np.random.randn(6,)
#print(q)
ur10.set_joint_target_positions(q)
# raw distance readings
base = expr.get_proximity_reading_from('Base')
elbow = expr.get_proximity_reading_from('Elbow')
tool = expr.get_proximity_reading_from('Tool')
# 3d point readings with id
base_pc, base_obj = expr.get_points_from('Base')
elbow_pc, elbow_obj = expr.get_points_from('Elbow')
tool_pc, tool_obj = expr.get_points_from('Tool')
base_obs = np.hstack([base_pc, base_obj])
elbow_obs = np.hstack([elbow_pc, elbow_obj])
tool_obs = np.hstack([tool_pc, tool_obj])
complete_obs = np.vstack([base_obs, elbow_obs, tool_obs])
# human position
hum_position = np.array(expr.get_human_position()).reshape(1, 3)
if len(buffer) == 1:
buffer.popleft()
else:
buffer.append(complete_obs)
if len(buffer) > 0:
buff_stack = np.vstack(buffer)
socket.send_pyobj([buff_stack, hum_position, q])
pr.step()
sim_step += 1
# np.save('data.npy', np.vstack(data_store))
# np.save('gt.npy', np.vstack(ground_truth))
rtd.stop()
pr.stop() # Stop the simulation
pr.shutdown() # Close the application
import sys
sys.exit()