def get_joint_position(self):
'''Get joint position using streaming communication.'''
joint_pos = [
vu.get_joint_position(self.client_id, h)
for h in self.handles_dict["joint"]
]
return joint_pos
def get_handles(self):
client_id = self.client_id
_handle_by_name = lambda n: vu.get_handle_by_name(client_id, n)
joint_names = [
"Prismatic_joint_X",
"Prismatic_joint_Y",
"Prismatic_joint_Z",
"Revolute_joint_Z",
"Revolute_joint_Y",
"Revolute_joint_X",
# "Prismatic_joint_Fing1",
# "Prismatic_joint_Fing2"
]
joint_handles = [_handle_by_name(name) for name in joint_names]
ft_names = ["FT_sensor_EE"]
ft_handles = [_handle_by_name(n) for n in ft_names]
'''
ee_names = ["EndEffector"]
ee_handles = [_handle_by_name(n) for n in ee_names]
ft_names = ["FT_sensor_Wrist", "FT_sensor_EE"]
ft_handles = [_handle_by_name(n) for n in ft_names]
cam_rgb_names = ["PalmCam_RGB"]
cam_rgb_handles =[_handle_by_name(n) for n in cam_rgb_names]
cam_depth_names = ["PalmCam_Depth"]
cam_depth_handles =[_handle_by_name(n) for n in cam_depth_names]
'''
robot_handles_dict = {
"joint": joint_handles,
"ft": ft_handles,
# "EE": ee_handles,
# "FT":ft_handles,
# "RGB":cam_rgb_handles,
# "Depth":cam_depth_handles
}
## Start Data Streaming
joint_pos = [
vu.get_joint_position(client_id, h) for h in joint_handles
]
'''
EEPose=vrep.simxGetObjectPosition(clientID,RobotHandles["EE"][0],-1,vrep.simx_opmode_streaming)[1];
EEPose.extend(vrep.simxGetObjectOrientation(clientID,RobotHandles["EE"][0],-1,vrep.simx_opmode_streaming)[1]);
FTVal=[vrep.simxReadForceSensor(clientID,FTHandle,vrep.simx_opmode_streaming)[2:4] for FTHandle in RobotHandles["FT"]];
RobotVisionRGB=[vrep.simxGetVisionSensorImage(clientID,sensorHandle,0,vrep.simx_opmode_streaming) for sensorHandle in RobotHandles["RGB"]];
RobotVisionDepth=[vrep.simxGetVisionSensorDepthBuffer(clientID,sensorHandle,vrep.simx_opmode_streaming) for sensorHandle in RobotHandles["Depth"]];
'''
return robot_handles_dict
def get_handles(self):
client_id = self.client_id
_handle_by_name = lambda n: vu.get_handle_by_name(client_id, n)
joint_names = [
"Prismatic_joint_X",
"Prismatic_joint_Y",
"Prismatic_joint_Z",
"Revolute_joint_Z",
"Revolute_joint_Y",
"Revolute_joint_X",
# "Prismatic_joint_Fing1",
# "Prismatic_joint_Fing2"
]
joint_handles = [_handle_by_name(name) for name in joint_names]
ft_names = ["FT_sensor_EE"]
ft_handles = [_handle_by_name(n) for n in ft_names]
octree_handle = _handle_by_name("Octree")
# vision_sensor = _handle_by_name("Vision_sensor")
robot_handles_dict = dict(
joint=joint_handles,
ft=ft_handles,
octree=octree_handle,
# vision_sensor=vision_sensor,
# "EE": ee_handles,
# "FT":ft_handles,
# "RGB":cam_rgb_handles,
# "Depth":cam_depth_handles
)
## Start Data Streaming
joint_pos = [
vu.get_joint_position(client_id, h) for h in joint_handles
]
return robot_handles_dict
def run_scene(self, scene_i, save_dir):
self.scene_info = {}
args = self.args
logging.info("Start sim: {}".format(scene_i))
self.reset()
self.step()
# Generate anchor handle
anchor_pos = [0.0, 0.0, 0.0]
anchor_args = self.get_anchor_object_args()
anchor_handle = self.generate_object_with_args(anchor_args)
for _ in range(4):
self.step()
# Generate the other object
other_pos = [0.0, 0.0, 0.0]
other_obj_args = self.get_other_object_args(
other_pos, obj_size=[0.04, 0.04, 0.04], add_obj_z=False)
other_handle = self.generate_object_with_args(other_obj_args)
for _ in range(5):
self.step()
# Move other obj to the right place
anchor_abs_bb = self.get_absolute_bb(anchor_handle)
anchor_lb, anchor_ub = anchor_abs_bb[0], anchor_abs_bb[1]
# other_obj_pos = self.get_other_obj_position(
# anchor_args, anchor_pos, anchor_lb, anchor_ub,
# other_obj_args['obj_size']
# )
new_other_obj_pos = self.get_other_obj_location_outside_anchor_cuboid(
anchor_args, anchor_pos, anchor_lb, anchor_ub,
other_obj_args['obj_size'])
if new_other_obj_pos is None:
logging.info("Cannot place other obj near anchor.")
return
print("Should move other obj to: {}".format(
_array_to_str(new_other_obj_pos)))
anchor_pos = self.get_object_position(anchor_handle, -1)
logging.info("Anchor pos: {}".format(_array_to_str(anchor_pos)))
joint_handles = self.handles_dict['joint']
joint_pos = [
vu.get_joint_position(self.client_id, h)
for h in self.handles_dict["joint"]
]
logging.info("Current joint pos: {}".format(_array_to_str(joint_pos)))
self.set_all_joint_positions(new_other_obj_pos + [0, 0, 0])
for _ in range(10):
self.step()
joint_pos = [
vu.get_joint_position_oneshot(self.client_id, h)
for h in self.handles_dict["joint"]
]
logging.info("New joint pos: {}".format(_array_to_str(joint_pos)))
old_pid_values = self.get_joint_pid_values()
# self.set_joint_pid_values(10.0, 0.0, 1.0)
'''
self.set_diff_joint_pid_values([
[0.01, 0, 0], +1.0000e-01
[0.01, 0, 0],
[0.01, 0, 0],
[0.05, 0, 0],
[0.05, 0, 0],
[0.05, 0, 0],
])
'''
new_pid_values = self.get_joint_pid_values()
logging.info("Old pid values: {}\n"
"new pid values: {}\n".format(
_array_to_str(old_pid_values),
_array_to_str(new_pid_values)))
# Now try to perform actions
# Try to go down for now
if TEST_MOVE_IN_DIR == 0:
after_action_joint_pos = [
joint_pos[0], joint_pos[1], anchor_pos[2]
]
elif TEST_MOVE_IN_DIR == 1:
after_action_joint_pos = [
joint_pos[0], anchor_pos[1], joint_pos[2]
]
elif TEST_MOVE_IN_DIR == 2:
after_action_joint_pos = [
anchor_pos[0], joint_pos[1], joint_pos[2]
]
self.toggle_record_ft_data()
self.step()
self.set_prismatic_joints(after_action_joint_pos)
for _ in range(5):
joint_pos = self.get_joint_position()
print(_array_to_str(joint_pos))
self.step()
'''
self.free_other_object()
self.set_joint_pid_values(0.1, 0.0, 0.0)
for _ in range(20):
self.step()
curr_joint_pos = self.get_joint_position()
self.reattach_other_object()
self.set_all_joint_positions(curr_joint_pos)
'''
for i in range(50):
self.step()
if i % 5 == 0:
joint_pos = self.get_joint_position()
print(_array_to_str(joint_pos))
ft_data = self.save_record_ft_data()
assert ft_data is not None
ft_data_arr = np.array(ft_data).reshape(-1, 6)
ft_mean = np.mean(ft_data_arr, axis=0)
logging.info(
bcolors.c_red("Mean ft value: {}".format(_array_to_str(ft_mean))))
return True
def run_scene(self, scene_i, save_dir):
self.scene_info = {}
args = self.args
logging.info("Start sim: {}".format(scene_i))
self.reset()
self.step()
# Generate anchor handle
anchor_pos = [0, 0, 0.1]
anchor_args = self.get_anchor_object_args()
anchor_handle = self.generate_object_with_args(anchor_args)
for _ in range(4):
self.step()
# Generate the other object
other_pos = [0, -0.1, 0.0]
other_obj_args = self.get_other_object_args(other_pos, add_obj_z=False)
other_handle = self.generate_object_with_args(other_obj_args)
for _ in range(5):
self.step()
# Move other obj to the right place
anchor_abs_bb = self.get_absolute_bb(anchor_handle)
anchor_lb, anchor_ub = anchor_abs_bb[0], anchor_abs_bb[1]
# other_obj_pos = self.get_other_obj_position(
# anchor_args, anchor_pos, anchor_lb, anchor_ub,
# other_obj_args['obj_size']
# )
new_other_obj_pos = self.get_other_obj_location_outside_anchor_cuboid(
anchor_args, anchor_pos, anchor_lb, anchor_ub,
other_obj_args['obj_size'])
if new_other_obj_pos is None:
logging.info("Cannot place other obj near anchor.")
return
print("Should move other obj to: {}".format(
_array_to_str(new_other_obj_pos)))
anchor_pos = self.get_object_position(anchor_handle, -1)
logging.info("Anchor pos: {}".format(_array_to_str(anchor_pos)))
joint_handles = self.handles_dict['joint']
joint_pos = [
vu.get_joint_position(self.client_id, h)
for h in self.handles_dict["joint"]
]
logging.info("Current joint pos: {}".format(_array_to_str(joint_pos)))
self.set_all_joint_positions(new_other_obj_pos + [0, 0, 0])
for _ in range(10):
self.step()
joint_pos = [
vu.get_joint_position(self.client_id, h)
for h in self.handles_dict["joint"]
]
logging.info("New joint pos: {}".format(_array_to_str(joint_pos)))
old_pid_values = self.get_joint_pid_values()
self.set_joint_pid_values(10.0, 0.0, 1.0)
new_pid_values = self.get_joint_pid_values()
logging.info("Old pid values: {}\n"
"new pid values: {}\n".format(
_array_to_str(old_pid_values),
_array_to_str(new_pid_values)))
# Now try to perform actions
# Try to go down for now
after_action_joint_pos = [
joint_pos[0], joint_pos[1], anchor_pos[2], 0, 0, 0
]
self.set_all_joint_positions(after_action_joint_pos)
for _ in range(5):
self.step()
self.free_other_object()
self.set_joint_pid_values(0.1, 0.0, 0.0)
for _ in range(20):
self.step()
curr_joint_pos = self.get_joint_position()
self.reattach_other_object()
self.set_all_joint_positions(curr_joint_pos)
'''
self.step()
joint_pos[2] += 0.2
self.set_all_joint_positions(joint_pos)
other_pos = self.get_object_position(other_handle, -1)
logging.info("Other pos: ({:.4f}, {:.4f}, {:.4f})".format(
other_pos[0], other_pos[1], other_pos[2]))
# Move to slightly above the anchor block.
offset = np.array([0.0, 0.0, 0.00])
# above_anchor_pos = anchor_pos - other_pos + offset
above_anchor_pos = anchor_pos + offset
logging.info("Move to pos: ({}, {}, {})".format(above_anchor_pos[0],
above_anchor_pos[1], above_anchor_pos[2]))
self.set_all_joint_positions(above_anchor_pos.tolist() + [0, 0, 0])
self.step()
# self.set_object_position(other_handle, above_anchor_pos.tolist())
for _ in range(10):
False self.step()
joint_pos = [vu.get_joint_position(self.client_id, h)
for h in self.handles_dict["joint"]]
logging.info("Joint pos: {}".format(joint_pos))
# self.set_object_position(other_handle, anchor_pos.tolist())
# self.set_object_position(self.handles_dict['ft'][0], anchor_pos)
'''
for _ in range(50):
self.step()
return True
def get_joint_position(self):
joint_pos = [
vu.get_joint_position(self.client_id, h)
for h in self.handles_dict["joint"]
]
return joint_pos