def set_position(self, position: List[float], relative_to=None,
reset_dynamics=True) -> None:
"""Sets the position of this object.
:param position: A list containing the x, y, z position of the object.
:param relative_to: Indicates relative to which reference frame the
the position is specified. Specify None to set the absolute
position, or an Object relative to whose reference frame the
position is specified.
:param reset_dynamics: If we want to reset the dynamics when moving
an object instantaneously.
"""
relto = -1 if relative_to is None else relative_to.get_handle()
if reset_dynamics:
for ob in self.get_objects_in_tree(exclude_base=False):
ob.reset_dynamic_object()
vrep.simSetObjectPosition(self._handle, relto, position)
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_dummy.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()
pr.step()
time.sleep(5)
data = np.load("/home/sarthak/PycharmProjects/research_ws/data/real-data/dataset14.npy")
data_store = []
#ctr = 0
while True:#sim_step <= STEPS:
#q, _ = rtd.get_joint_states()
q = data[sim_step][51:57]
q[1], q[3] = q[1] + 1.57079, q[3] + 1.57079
hum_pose = list(data[sim_step][-3:])
# ur10.set_joint_target_positions(q)
#q = np.random.randn(6,)
#print(q)
ur10.set_joint_target_positions(q)
hum_pose[2] = 0
vrep.simSetObjectPosition(expr.get_object_handle("Bill"), expr.world_handle, hum_pose)
# 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_pose, q])
pr.step()
sim_step += 1
time.sleep(0.008)
rtd.stop()
pr.stop() # Stop the simulation
pr.shutdown() # Close the application
import sys
sys.exit()