Beispiel #1
0
    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
Beispiel #2
0
 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))
Beispiel #3
0
    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]
Beispiel #4
0
    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)
Beispiel #5
0
 def get_object_handle(self, object_name):
     return vrep.simGetObjectHandle(object_name)
Beispiel #6
0
    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)))
Beispiel #7
0
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()